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Tne passive target motion analysis proDiem is oefinec as :ne estima- 
tion of tne velocity ana position of a target DaseO on Dassr.-e noiSu. 
measurernems acguirec from sonoDuoys. SonoDuous are capaDle :f re- 
ce’viPiQ a noise corruptee .acoustic sionature emitteu l-u s tdi get. m; s 
signaiiire is transmitted to an Anti-Suomarine v/arfarenA5Vv; aircraft 
wnere various comDinations of aroDiitude, frequericy. bearing anc time 
delay information can be obtained. Two of the Drimary information 
sources used for passive target tracking are relative Doppler from tne 
target's radiated frequencies and bearing information from directional 
sonobuoys. If the sonobuoy is a Directional Finding Acoustic Receiver 
(DIFAR) buoy, a bearing can be obtained from the buoy to tne target, if the 
sonobuoy is either a DIFAR or a LoVf' Frequency Acoustic Recei/er (LCFAR- 
buoy, the frequency spectrum of the acoustic signature can be determined. 
These two types of noisy measurements are noniinear functions of tr.e 
target's position, course and speed. 

The purpose of this research v/as to develop an operationai aidoritn.-v. 



u imU c-uuiTId; niec- li uin Jij auL/uLfiiiti i Jvi :T) f.i 



!U 



n f 



noisy measurements. This algorithm assumes a ciose tracKing : 

mciJi « poLMy. n» Ui uc?l lu up^dl dec, Liit: . 

initiai estimated position, course, speed, frequeroj and an area of proo- 
ability for the target of interest, in a close tracking environment this 
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inr'orrna' ion Vvoui-j De avaiiaDie inrougn human oDeraiora, ^•?-Cr’VrG rrarr 
oihcr tracr. ing routines, or recsivec I'rom ctner exiernai sensors. 

Since tne noisy measurement eouaiions are noniinear, noniinear filter 
theory must he applied to the tracking algorithm. The Extended Kalman 
filter theory represents, in principle, an ideal solution to this type of 
proPlem, The f ilter provides 

1. For the use of any numDer, combination and sequence of external 
measurements. 

2. its ov^n error analysis. 



A 



Structure v/nicn is recursive. 

4. The aDility to reconstruct the entire state vector from a noisy 
iTieasuremient . 

3. ,A f orm of 1 inear izat ion 

The discrete time version of the Extended Kalman filter is de'.’eloDed m 

I i < I I c I Vw’M cI3» < w V '/ 'U r«» v^-i i L*‘ 1 I u/ c 'i..* O u c Wi V • I c p- 1 ^ v o 1 i i w * ^ i 

comiputers. 

Before the filter theory of Kalman can De applied to the proDiem, the 
system and measurement models must be developed. From the geometry of 
the target-sonobuoy problem, the target's equations of motion and noise- 
free measureiTient equations are defined in Chapter II. 

In Chapter 111, the. theoretical background and assumptions used in 
deri’.-’ing the discrete Extended Kalman Filter are presented. The discrete 
Extended Kalman Filter is then applied to the passive acoustic track me: 



pr uD iem. 



in 



the final subsection of Chapter ill. the problem 



maneuvering and divergence control is presented. System modeling errors 
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occijr vvnen tne target undergoes a maneuver. These modeling errors may 
cause the filter to diverge, in order to prevent divergence and to accom- 
iViOdate maneuver, an adaptive control method is developed. 

in Chapter iV, the concept of error ellipsoids is developed. Error 
ellipsoids provide significant information aocut me estimated r.03;t:cn. 
Also, the error ellipsoids are useful in visual izing the estimation error. 

Chapter V discusses the development and implementation of me muit’- 
pie sonoPuoy tracking algorithm. The algoritnrn is divided into tnree 
modules. 

i. The track rnoduis Generates noise-free track data. 



2. The oDservation module receives the noise-free track data generated 
Du the track module and simulates noisy iTieasurement data lor input 
to the tracking algorithm module, 

5, The tracking algorithm module receives noisy measurement data and 
generates estimates and predictions. 

in Chapter VI, five scenarios are presented to demonstrate the per- 
formance of the algorithm. The final Chapter summarizes the results of 
this research and presents the conclusions and recommendations Cor 
turtner study. 
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II. PRCei.Fn STATEnFNT 



r-,. r 
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A. SYSTEM MODEL 

Consiaer tne t3rget(suDmarine)-oDserver(3GnoDucy) encounter 
rvyo airnensionai piane as shov/n in Figure (2-1). [Ref. I'-p. 5] The x and u 
components of the target yelocity, are denoted vcr snd v/nile tre > 
and y components of the target and observer positions are denoted x^, y^. 
X| 5 , and y'c) respectively. The course of the target is Bt and tne bearing 
from the observer to the target with respect to North is 0. Tne distance 
from the observer to the tar Get is 



f ~ ^ V A f A hj ,/ ~ 



(Uf -Uh.r 



Since the doppler shifted frequency is used as an oDservaoie. it is also 
necessary to estimate the rest frequency of the emiitter on the target. In 
this North-East oriented Cartesian coordinate system, a fifth order state 
v.ar lab ie is chosen. 
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rnO'ion. The noiai ion usee tnrouqnoui tiie incOis is mat vocnor ouanvuos 
aro iowGr cass isttsrs anc unCGrliPicC ano iTibiriCGS arG gi'/gm uoC'Sr v-asr 




1 . DiscretP Tirnp Fstimation 



ine aovarcGS in (ligirai cofT‘puter' "ecrinok'iny ano OGvGloDPPGn^ 

^'^alnian iruo tGO-iiiiiOUGS fria-G It pL'SSiDiG t uDtam a StfaiU’p ' ''V “i 0 

PGClir'Slvp SOiUtlOn tO 8 r'Gai'tirOG GSt IP'iat lOn pr‘ODiGn'1, Ti'ifr'G tOpGS O’ 

Gsiiniatjon pr’ijDlGrris ai'G snown g*i oiqurc (2-2}. V'Toa aijs'G''!' 

invotvps Gsi irnat ino ihg sialG variaDlGS of ihG sustGrri ai iwtig t.: doShOi 



upon a sequence of oDservations taken up to an including time t, . Tno 
problem is termed liltering when tt< - tj : smoothing when ti,. < tj ; and 
prediction when ti,. > tj. [Ref. 2-p. 5] Since the primary purpose of this 
work is to develop an algorithm for tracking suhrriannes. only the fiU.erinc 
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and Dredicvion DroDierns are consiasrea. Lei me lime iniervai oeiween 



discrete time t^ and ti,+, be defined as 

• • “ W+l ” ' 



(a) 



(2-3) 



DENOTES SPAN OF AVAILABLE 
MEASUREMENT DATA 



'*k= tj 
FILTERING 



(b) SMOOTHING 



•j 



(c) PREDICTION 

Figure 2-2 Three Types of Estimation Problems 
(estimate desired at time ti,) 



2. Target Maneuvers 

it is assumed that all maneuvers are imparted by white random 
’■‘crcing fu'xtions, as depicted in Figure (2-5), let the random variaoies 
and <5'^^ represent the following' 

- acceleration along the target's course 
= angular velocity or turn rate 
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Also let tne random variable <?fo represent the change in frequency, ihe 
quantities <^et -^^d dfQ are random changes of the target which are 



assumed to be independent and zero mean 
= expected value of 

= - El^foU^)! -0 



for all 0 



The variances of dyt. <^et> are define by 







E[*', 

Ei^ 



A < 






1 

j » 



! ^ -* 



c: 



Funner it is assumed that the random variables remain constant Over the 
discrete time interval T (i.e., a random walk). The followinq values for 
the standard deviations were taken from estimated rnaneuverind 
parameters for the target. [Ref. 3:p. 39] 
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Lr •. /T *** • •* * L f ^ 

= G.’. aecrtii/sec 
(Tt'n - O.GG i riZ'''sec . 

H 0 r )C 9 I n 0 V ci *' 1 8 nc 9 S 8 1' 9 

<7;^^ = (O.Olkts/sec)^ = 410,8 yds^/min^ 

<7et^ = (0. ldeg/sec)2 = 0.01096 rads'^/m in- 

= (O.OOlhz/sec)^ = 0.0036 hz'^/rn in- 

These values are used in uie scenarios in Section vi . 

Using the definition of T and the equations of motion in the x-y 
Diane, the difference equations can be obtained from Figure i2-0 and 
Fiaure (2-5) 



\ / / 



x(k+ri = 



Xf(k,+ 1 ) 




x^fk:) + T'V^t(k) ^ g^(d\,r,detXf 


+ 1 ) 




v.,4k j ' 


Li/k'- 1 : 

,^1, 
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yyiki + T-VytCkj - 


V/ 1 ) 
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i **,v N J / 

fc ^ • 4 




fo(k) - g^i^fo) 



The random forcing function terms g^ trough 05 are included tc 
account for random changes in speed, heading and frequency /vhich car 
occur for a maneuvering target. Writing equation (2-8) in more familiar 
terms 
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Vy'i'l'pr c 



v/i represents the rate of change of speed and heading with 
respect to the x component. 

W 2 represents the rate of change of speed and heading with 
respect to the u component. 

W 3 represents the rate of change of the freciuencu 
component. 



Hence, our systemi rnooei is in the linear form 



x(k+]) = 4^(k-) • x('k) + Hk) • w(k) 



( ■ ' h I 



if e 



x(k) i.s the N x 1 dimensiona! state vector. 

vik) IS the N X N dimensional state transition mativx, 

vvfk) is the M X t dimensional vector of random forcing 

TurKCtions, 

n’k) is the N X M dimensional state forcing rriatrix. 
k is the discrete time index representing time ti,. 



B. NOISE-FREE nEASUREHENT EQUATION 

AS a vessel moves through the ocean, it radiates an acoustic 
signature, information about the vessel can be obtained, if the sonobuoy 
receives the acoustic signature. As indicated in section !, a bearing to 
rov '•irTior can oe derermined from a DIFAR sonoDuou. "Fhe tveduencu 
spectrum of the acoustic signature can be found, if tne sonobuoy is eitner 
a difar or lofap. The ooppier effect relates tne chance m 'eceivec 
fredueix'u at a sensor to the relative motion between the '^arcet anc 



sensor. Thus from the received acoustic sidnature. two C!f‘erent types 0 ^ 
measurements are available for determininc position, course, anc speed 



the target, in C 
Doppler equation, 



artesian coordinates, from Ficure (2-i) t.ne nnise- 
[Ref. 4:p. 258] and [Ref. 5^p. 24] may be written .as 



f! 
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wn^r ^ 



r,, ■ Vj 



)-v^, f Oj,-yc)'V,j, 



I *' 1 ^ ' 

U' s ^ j 



V (ut-Ub)- 



Vr, '.s the SDeec of sound in the water. 

K 

I., IS the frequency naaiateo Dy me target. 

sin 0t IS the speed along the x axis, 
Vyt = v^* cos 6t is the speed along the y axis. 



For underwater tracking the observed doppler shifts are quite small. 
The speed of sound, v^, is approximately 3000 knots in the water so a 5 
knot target has a velocity/speed of sound ratio of 5/3000 or 
approximately 0.16^. This corresponds to t 0.5 hertz for a radiated 
frequency of 300 hertz. 

The noise-free bearing equation obtain from Figure (2-1) is 



0 = tan-1 



Ut “ Ub 



(.2 - 1 2 ) 



rv j 



larr^^ } is the inverse tanaent function. 



C. MULTIPLE 50N0BU0Y PROBLEM 



Fiqure (2-4) 



[Ref. 4 y>. 2561. 



illustrates the geometry of 
As discussed in Subsection 



the multiple sonobuoy problem 
li,B, a DIFAR buou can provide 



a bearing and frequency measurement. While a LOFAR buoy can provide only 



a frequercy measurement. The reference point in 
represent any iatitudeCy coordinate) and longititude(x 



Figure (2-4) can 
coordinate). The 
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targei's position and sonoDuoy positions are in relation to tne reference 
point. Hence, the latitude and longitude for any of these positions can 
easily De oDtained. 




ref. posit. 

Figure 2-4 Multiple SonoDuoy ProDleavGeometry 
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The purpose of the Kalman filter is to keep track of the state of the 
system by means of a sequence of noisy measurements. Conceptually, this 
technique may be viev/ed as a rr.ethod to systematically recuce the 
measurement errors associated with the observations of the target's 
acoustic signature to determine an optimal estimiate of the target's 
position and motion. 

When both the system and measurement models are linear functions of 
the state variaoies the halmian filter is the appropriate technique to use. 
As given in section H. tne discrete time Kalman filter dynamic syste:'!" ’s. 
described by the state equation 

x(k*]) = 'l>(k) • x(k) + F(k) • w(k) i2-l0) 

nd the measurement equation is related to the state oy 

z(k) = H(k)'x(k) + v(k) (3-1) 



Q. 



'Where 



x(k) is the N x 1 dimensional state vector. 

0(k) is the N X N dimensional transition matrix, 
wfk) IS the h X I dimensional vector of random forcing 
functions, 



IS the N X fl dimensional state forcing rnatrix, 
IS the X 1 dimensional measurement vector, 
HO') IS the .H’ X n dimensional measurement matrix, 
v(.k) IS the P X I measurement noise, and 
k IS the discrete time index reDresentina lime ti,. 



C’ TLiC r vTr^vinrr--. ;/ man* pi{ 'Tcn 
u. i mL t»'N \ i-iNutu \r^\H I iL J L.n 

Looking ai equations (2-11) and (2-12) it can De seen that our meas- 
urement equations are nonlinear functions of the state vanaoles. The 
nonlinear measurement miodel oecomes 

;(k) = H(x(k).k) - y(k) (3-2; 

where the measurement, z(k). is a function, h(x(k),k) of the state vanaoles 
Dlus some noise (i.e. error), y(k). The h(x(k),k) must De "lineanzea.’' This 
IS accornplisned Dy expanding h in a Taylor series aDout the Dest estimate 
of the state at the time and only the first order terms are feot [Ref. 
3-P.34] and [Ref. 2-p.lS2]. The application of the Kalmian filter to the 
nonlinear case is called the Extended Kalman filter. Higher order, more 
precise approximations to the optimal nonlinear filter can De acneived 
using more terms of the Taylor Series expansion for the nonlinearities, 
and Dy derviving recursive relations for the higher moments of state 
variaDles. For a derivation and additional discussion of Extended Kalman 
filtering the reader can refer to [Ref. 2-pp. 180-225] or [Ref S^pd. 219- 
292], The following will De a summarized discussion of the discrete 
Extended Kalman filter equations. The linear form; of equation (3-2) yields 

z(k.) = H(k)-x(k) y(k) 



V. w' '-4 > 



Yvn^rr 9 



H(k) = 0n(x(k),k) 
8xfk) 



j>:(k) = OKik-i) 



xtk; is the M x 1 dimensional estimiate state af^er me ktn measurerneni. 
x(f-ir-i) IS the N-oirnensionai predicted values of the state vector oefore 
the kth measurement. That is 



x(k|k-l) = $(k)-x(k-l Ik-1) . 



fr .-I ' 
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Tne measurement noise is zero mean ano is uncorrelaiea vriu 



covariance n(k) 



u[v(k)] = 0, for all k^O 



E[v(k)'V( I) 



Ti - 

J - 



0, k'»J 
R(k). k=j 



(5 -5a) 
(3-5b) 



= R(k)-5|<j, for all k.j ^0, 

rtie random forcina fuixiion is zero mean and uncorrelated with 
covariance Q’(k) 

Etw(k)] =0, foralik^O 
E[w(k)'w( ).)'*’) = forallk.j^O. 



(5 -6a) 



5. Tne random forcing input and measurement noise are uncorrelaiea 
E[wfk)-v(j;C = E[y(j)-w(j,n = 0 for all k,j> 0. (3-7) 

4. The initial state is a random variable v/itn knov/n mean and 
■yanance 



EiX(O)] -aq , 



and 



E{[x(0) - ijHxCO) - X,/} = P(Oi-l) = F'o . 13-'?) 



5. The initial state and measurement noise are uncorrelated 

E[x(0)-v(k)^l = E[v(k)-x(0)^l = 0 ,forallk»0. (3-iO) 

6. The randoiTi forcing input and initial stale are uncorrelated 

E[w(k)'x(0)^l = E[x(0)-w(k)^l = 0 for all k^ 0 . (5-i i ) 



The state estimation error vector 3^*k) is define by the estimated state 
vector minus the true state vector 



I . V 



M k j = XI. >' ! k; ) - xi k 1 , ^ 5 - I 2 1 

and ine predicted state estimation error vector is defined Du the predicte. 
state vector minus tne true state vector 

X 1 n I K “ 1 ) — x( k j k ~ 1 ) ~ K I 

I ne covariance oi estimation error matrix is Given Du 
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ano me Dreaicma covariance oi state error mainx :s 

P(k|k- 1 ) = E{£(k|k-l)-x(k|k-l)^} ( 3 - 15 ) 

The state excitation covanaixe matrix is given hy 

Q(k) = r(k)-E{iv(k)-w(k)'^)T(k)^ (3-16) 

= r(k)-Q’(k)T(k)T , 

if the estimate is selected to have the form 

x(k I k) - x£k j k“ 1 ) G(k,)'[z(k) ■ h( x£k | k~ 1 ))j ( 5 " 1 7 ) 

anc the ootuTfal estimator is defined as the one that rriinirnizes the sum of 
the variances of estimation error, i.e, 

E {X|(k)2) + E {X 2 (k) 2 } + + E {£,(k) 2 ) ■ 

then the ODtimai estimation gams are those which satisfy the eouations 
0 (k) = Pfk|i:-IJ-H^(k) ■ [H(k')'P(k lk-l)'H%) *R!k)J-' ( 3 -iS> 

P(k'|k) = [l-OLk)-H(k)) • P(k|k-i) 

P(k» I I kj = '{■(k)-P(k I * Q(k) 

If the estimation equstion (3-17) is initialized with the value 

x(0|-i) =0,, 

it can be shown that the optimal estimate x(k|k) is unbiased i.e. 

E{[x(k|k)-x(k))} = 0 forallk>^ 0 , ( 3 - 22 ) 

and the initial condition is 

E{[x( 0 )-i,))[x( 0 )-Xon = P( 0 |-l) = P„ , ( 3 - 23 ) 

in surniTiary. lahie o“i detines the discrete Extended Kalman tiiter al- 
gorithm for the linear state eouation ( 2 - 10 ) and the linearized measure- 
ment ecuation ( 5 - 5 ). Eduations ( 5 - 17 ). ( 5 - 13 , ( 3 - 19 ), ( 3 - 4 ;, and v 5 - 20 ) 
con'iDrise the Extended Kalman filter recursive eguat ions. [Ret, Z'-o. 19 G| 
Once the looD is entered, it can De continue ad infinitum, m onnCiOie at 



I ^ ‘ 



f3-"o 

V w . 



( 3 - 21 ) 
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T Deriin^nt. ecjuaiions and me sequence oi compuiaiionai 



o L c •k' : 



ari5 snown in Figure (3-i). 



TaDie 3-1 smnARYOF kalhan equations 



SYSTEM MODEL: x(k» i = 'J>G,ML) - r(k)w(k) 

wnere w(k) ~ N[0, Q'(k)l 


(2-10) 


MEASURE MODEL: Z(k) = MA(k),k) * v(k) 

wnere y(k) ^ NIH R(k)] 


(3-2) 


INiTiAL tSONDlTiONS: x(0) ~ N(x, , E,] 

OTHER ASSUf'iPATlONS: E[w(k)-y(i)'’'i = 0 for all k.j 


arid 

lA (3-C •> 


6A IN EQUAT iON- 

G(f") - P'G' j ^ j’!N(!<)*P(k j k - 1 j ■^H(k 


;)H (3- NS: 


ERROR covariance UPDATE E^ 
P(k|k) = [l-G(k:)‘H(k.)]'P(k 


X'ATION: 

k-1) 


(5-19J 


STATE ESTIMATE UPDATE EQUATION: 

x(k [ k) = x(k [ k“ 0 * 0(k)'[z(k) - li(x(k j k- ! ))] 


(5-17) 


ERROR COVARIANCE PROPAGATION EQUATION: 
P(k»! |k) = 'I<k)-P(k|k)-'I'(kn » Q(k) 


1 

ro 

o 


STATE ESTIMATE PROPAGATION: 
x(k- i 1 k) = $!k)- x(k i k) 


(3-H.) 


j I 4 i j i • r> K / 


).k) ! 






x(k) = x(kik-i 







3“ 1 F-Blnridn Filter Rpcursiv^ LOOD 



C ? 1-’"^ r iC N'' ^ Tn < re ^ I"*. C;'M i a ^ r'' ^ iC 

i ^.1 i 1 w. J i ^-' . i I ni'^.i-«>, 4c. I 
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an filter to tne passive acoustic tracking proDlern will oe ciscussecl. 
Given the system anc noise-free measurement models developed in Section 
li, vy'e will derive the random forcing function w(k), the state excitation 
covariance matrix Q(k), the measurement equation z(k), and the 
measurement noise covariance matrix R(k). 

I . Random forcing function 

From equation (2- ID) the vector w(k.) represents the effect on tne 
states of the random forcing function and may he calculated from equa- 
tions relating ana to tne target heading. 0^. and velocity, '/>. F^om 
Figure (2-1} the velocity in the x direction is 



.‘.f - Vj,.|. Vr ' sin 0^ , 
Differentiateted equation (2-9) qives 



( V » ' ; , 



yvnere 



■<t - - Vt ' 8+' cos Vt ' sin 8t 

sin 8f = v^-t 

cos 8t = . 

Vf 



(3-20. 



.eUinq -"ina ono suDStituting into equation (5-23), U^e 

jcc e 1 e r a T. 1 0'p o e'corn e s 



w. 



- V f 't Qt + i V r ! 
■'yt ' "Dt ■ ^ ^t' 



'•q 



6.> 

vV 
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Similarly, 



yt = Vyj = • cos 8t . and 

Ut = Vy* = - VfSt'Sin 8f + • cos 8t 

and afidr suDstitution 



vVo iKi ~ Ut '^y.t ’ ^6t ^ ^ vt 

Tne freduencu term r-ecornes 



(3-27) 

(3-28) 



1,0-^ 



V. 



Wt (>• I = r,_- = 6fo . '3- 

F‘"om ■r'd^-aiion. i2-*4) anc tn<e assumptions on the o’s. we conclude 
’s zero mean 

EiMK.)] = 0 . 

The ranaom forcing lurvotions covariance matrix is 
Q'(>:) = E[w(k)V(k)l 



E(wi(k)-Wo(i<)r E[w,(k)-W 3 (k)] 

E[w2k)2] ^ aw2(k)-W3(k)] 

E[w5(k)‘W-2(k)] EfwjCk)^] 



T Z*" 



fE[w,(k)2] 

E[w2(k)'W,(k)l 

E[w3(k)'W,(k)] 



(3- 



Since Svt- Set- ^nd Sfo are independent and zero m.ear 
E[w,(k)-W3(.k)] - E[w3(k)-w,(k)] = 0 



0( lu 



EiWTtkj-Wrf.k)] = E[v,'5(k)'V/2(k)] = 0 



V ^ 






f > 



) 0 ■ 



of w,( 


kj, v/ 2 (k), and ' 






u 




2 

1 0 


<N| 

''k> 

LU 

ill 


\u 


= E[yv/k)-W2l 
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SuDStiUiting eouation (3-25; for one canceiling 

V3ri3iiC0 of '('y'll.r. ) DOCOHitb 

\2 . cre 2i 2 . cf-r , 2i 



' ►-v r- ■*' r* • “» C C ^ •' r' C ^ 

. . :c ‘^.i ;rJ n,2. . .. IC 



'' T cru \ 

^ j 3 ju; 



rrofTi ^CLidtion tnG v3ri3r(C0 sirnDiinss to 

•-..2 z I'w i2 . . 2 ^ . 2 , 2 

V Wvt/ vt '^'4t 

Vr 

The variance of W2(k) and W3(k) can be found in similar fasnion to give 

' (3-35) 



Upon substituting for w/k), and W 2 (k), and simplifying the covariance term 
E[ w ,i> )• w^fk )] becomes 

Uxu = V.,, • Vy^ • ((Ovt^/Vt^) - 09t^) (3-37) 

'Substituting equations ('2-2 0 through (2-25) into equation (2-19) the 0’(k) 
matr>y rtesuit is 



0'(k) = E[^(k )] = 



2 • Pi 

X xy 



n * • 

'• •’<y '■ y 



0 



n 



r'j 

\y 



to 






/■•'iDr'O /■^••2 /-t.,2 

. I u ’ 



,0 are evaiuaiea at tne predicted values of 3,00 



” lT' 



2. State Excitation Covanaixe Matrix 

To compute the error covariance propagation equation (.3-19) the 
9 excitation covariance iriatrix Q(k) must be knovvn. The size of 0(k) 
has a direct bearing on the magnitude of the P(k+i |k) [Ref. 2-p. 76). The 
possibility of a maneuvering target and model inaccuracies are taken into 
account oy the state excitation covariance matrix. As more and more data 
is processed. Q(k; prevents the Kalman gams from approaching zero oy 
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ipisurinc some uncertaitay in tne Dfeaictec siate vecior. rrom O':]uaiion 
(3-16), ihe state excitation covariance matrix- 13 

Q(k) = r(k.)'Q'(k)T(k)T . (3-16) 

Substituting from equation (2-9) for r(k) and (3-38) for Q'(k) the state 
excitation covariance matrix is 



Oli.''! = 



4 



7 J.Q ..2 



XU 



.•1 

't 



. 

\ yjj 



0 



tZ ..^„2 

I V.y 



T2-0-a 









0 



* 

•1 



■^'0 






4 



I ,j 



0 



T^'0- 



T^-Ov? 



0 



0 



t4,,^..2 t3.,'c.. 2 f) 

• ^ H . - 



0 



0 






1 M I 



3. Measurement Eouation 

From equation (3-2) the nonlinear measurement equation is 
z(k) = h(x(k).k) + y(k) (3-2) 

.^5 indicated in equation (3-3a) and (3-3D) the linear formi of this equation 



n r ‘r* 



z(k) - Hi'i^i'X(k) v(k ; 

M( [■ I = nfsl vi'L- 1 Ir 'i 



cvy) I 



y( ) - v( I k' - 1 j 






i ^ ^ Ti I 



b'jD’StitutJnG the doppier equation (,2-! ! ) and oeannG equa 
nc^'ki.ki, the measurement miode! becomes 



on 1.2-' 2 i into 
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'I k ! = 



f ^ 1 
' .jU j 



0 I> ! 




Vfl K ) 



Vgil f 



i "-ar. 



Toh iinear izea rneasurerrient matrix is derived irorn equation 
r.n.ov/n DelOvv 



. — /r. . i ' 



ufV'^ - 
» y “ 



9f j(k 


k-1) 3fci(k i 


k-l) 3f^(k|k-l, 


) aokjk-l) 3fj(k| 


k-1) 


0x^(k 


7^ 

1 

< 

X 


|k-l) 3yt(k|k-l) 


9vyt(k|k-D) 8fo(k 


k-1) 



ae(k|k-i) 8 e(k|k-u ae(k|k-i) 9 e(k|k-i) aeik |k-i) 

^3x.fkik-i) 0v^i(k|k-l) 9y/k | k - 1 ) 3v,j((k j k - 1 ) ) 0fc,(k ] k - 1 ) 



; 3 - 4 l’ 



: 0 s I rn p ! ' t y not a t i on let 

u(k|k-l) = ^V30^■''l)''<t)('i^XVMk-^♦[yVk|kH)-Us(i)^■■ykjk-l) , (3-4;i 
and us mo 'romi eouation (u“i j the estimated ranoe 

- i K-l rxKini^-^iiJii'k I k- !)-!JhU)]“ . 



k - f i 






\^\\ i PiOP i * \ts t Oh X ifiO u corrinrinpr^"^^^' r?i i r^. SO'**-OC‘OOL' T : 

me Duoy pattern. 

For me freouerv'.ii friMesurk^menf , tne HicCk) component wiii De evaulaiec 
iirsT in order to reduce notation. Tne results of the partial derivatives of 
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me irequercij nne^sur emeni evaiuatea at tne Dreaic: 

V l' • I 1 _ 1 \ 

\ I r- i ‘ C 



c * Ti T ” C ' ;li - 

' ‘ J w' 'w» » >W* I * 





3fd(k 


1/ _ 1 
N 1 ) 


'’"p 




Hi.sik) - 


d(cC« 


' A 
N 1 ; 


= Vp + u(k k-l.) 


(5-44e) 



Let Ak = 






Ak' 



Hi '(k) = Ak- 



r(k|k-n 

f„(k|k-l)-(H,,5(l<)]2 then 
w iri'k lk-i)l=' 

v.^t(k i k - 1 )*r(k i k - 1 ) - u(k | k - 1 j'l'xVk | k - ) )-X|,/i)] 

r(k I k-l) 



r(k|kHHxV:K|k-l)-Xb(i)j 



5“H4aj 



(5-44D,' 



U J\^ 



/ I ' ' r 
y 



A 1 / . 



H ^ 4( k i - Mk ' 



'-kytCk I k“ 1 j*r(k j k" 1 ) - u(k j k - 1 )'[yt;(k | !<■ i ) i )] 

rlkjk-l)* 

V(k I k - ) )'[yf(k I k - 1 )-yt,(i)] 



/ 7 _ ■'I ^ > 

k ^ *7 



(3~H4di 



Simiiarly. tne partial derivatives of ine Deanng angle measure- 
ment evaluated at tne predicted state values x(kjk-i) follows: 



i('k) 


= [y,(k!k-l)-y,(i)! 


(3 -45a) 


-/k) 


= 0 


f5-45b) 


Ak) 

1 


- - [Xfik 1 k - 1 ,/i-Xp( i,)j 


!'3-45c) 


ik ) 
1 * 


= 0 


i -4“i(j ; 


c*l K J 


= 0 


'"3-^5e) 
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Tj ::omDij!.r the Kairnan filter gains, me rneasuremeni noise 
•.'anarrce matrix R(k) must oe known. From TaDle 3-i and equations m 
ana tne measurement noises, Vf(.k) ana Veikj are assumed to :e 
mean ano uncorrelaied. Tne noise covariance matrix is define as 






0 



w 



(3-T6) 



0 Oa 



2 



wnere Of is tne standard deviation of the frequency measurement noise 
Oe IS tne standard deviation of tne oearing measurement noise 

AS indicated Dy flitschang [Ref. 3-*p. 43], tne resolution of the 

mequency measurement is equal to tne inverse of the record length of the 

time siQnal. Various nurnDers from 0.02 to 1.0 hertz were testea in the 

simulations, a typical value r/ the frequency standaf'd dev>ai!on le 

o*' = 0.04 hertz . i’3-47,' 



f ) 



stanoa-'c aevramon value 's 
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se 1 s a 


^unCciOri 01 
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D. nANPijycp{M(3 akjq DIVERGENCE CONTROL 

An unprecise model may cause the filter to diverge. Divergence 
occurs When the calculated error covariance does not Dound the actual 
er>‘or covariance. In otherwords when tne calculated covariance mat'"’'*' 
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error covariance. In otnerworcs wnen trie caicijiateo covanance mafi/ 
oecornes too email or oDtimiistic. '.vnen tne calcuiatec ccvar-iance ma:.:": 
Decornes small, nence the filter gam is small. suDsecuent measuremients 
have little effect on the estimate. So the estimated state anc the actjai 
state diverge because the system model in the filter is different than the 



actual system model. Such model errors are due to the followings 

1. Approximations that might De made to simplify the filter 
comiputations 

2. Limited knosvledge of the physical system. 



3. Computational errors resulting from the use of finite precision 
arithmetic. 

In order to prevent divergerce and to accommodate maneuvering, tne 
basic idea is to increase the calculated covariance miatrix. Pfi 
model errors are compensated Dy a larger calculated covariance matn>;. 
Hovv'ever. this increase in the calculated error covariance m.akes the filter 






t r 



more sensitive to random errors in the 'mieasurernent crocess. 
resulting in poorer target estimates when the target does not unaergo a 
maneuver. As a result, an adaptive control method has been devised 
increase the calculated error covariance only when the target nas 
maneuvered. Further details on divergence is delineated in by Jazwinski 
[Ref.7HPp. 301-305] and Gelb [Ref. 2-pp. 277-3! 1]. 

The first procedure used to prevent divergence and to allo'w for ma- 
neuvering is the random forcing function w(k). As indicated in Subsec- 
tion iiI.C.2, Q(k.) prevents the Kalman gams from approaching zero by 
insuring some uncertainty in the predicted state vector. 
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The secoPiG. maneuver anc Givergerce control proceoure involves the 
aeveioprnent of an adaptive gate. From equation (3-i7) the difference 
between the actual measurement and the predicted measurement, is 
defined as the predicted measurement residual error (called hereafter 
predicted residuai) 

i(k h-'i) = - !i(xU:|k-l) , (3-4'j) 

Jazv/inski [Ref. ?:p. 271] provides the statiscal properties of the pre- 
dicted residua! Vf’hich are 

1 L'- 1 )] - Cj ' " -~n 1 



an 



-f 

nj 



Z'k |t-l ! ■ £[ea:|k-!-9(k|k-ini 

= H(k')'P(k|k-l)'H(k)'^ » R(k) , 

Therefore the predicted residuai standard deviation is defined as 






1 t 
t / 



o 



, = y Z(klk-i) = VH(k)-P(k|k-l)'H(kn • R(k) . (3-52) 



in order to allow for target maneuvers, we define an adaptive gate a: 
three times the predicited residuai standard deviation 



Gate3(rc) = 3'07 • 






t'/e can judge the performance of the filter by comparing the predicted 
f'esiduai to the adaptive gate. For each measurement the algorithm tests 
the residual and lets the residua! itseif determine the appropriate random 
1 ore I hid 1 unct i on 



!e(k 



k-i); < 5-a. 



-,4’) 



i he adaptive cate is adaptive in the foliowind sense. As iong as the 
predicted residuai remains less than the value, the random forcing 



32 
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Wh^-n the DreaicteG residuo! hecornes larger than the 3-c^ value tr,e :'i!:e" 
ia uivergiriu. To Drevenl oivercence the variances of the ranoorn forcmc 
fijnction (eouation (3-34).) are increased as fo!io>vs: 
c-v^ nsvy = lO'C*^^ old . 



V..* 

O’o^new = lO'O'y^old ,and 
Of,j^ new = 2 'O^^old . 



(3-55) 



The constants in equation (3-55) were found by trial and error. The 
random forcing function covariance matrix increases, which increases 
Q(k). When the. state excitation matrix, Q(k), iixreases, the covariance 
matrix. P(k|k-i) increases. The covariance matrix causes the adaptive 
gate to increase. Note H(k) and R(k) remain the same. Hence, the gate 
ODens the filter to the incomina measurement. At the next iteration the 



variances of the random forcing function reverts back to equation f 
to calculate random forcing function covariance matrix and the 
exicitation covariance matrix for the next measurement. 



jn i 



O I 



if the predicted residua! exceeds the adaptive gate three consec-juve 
riiTi05, tne aiuoritnrn is reiniu 3 iized to tne c^riGiCiai esprnateo error C0“ 
variance Hi.atri'<. ine reinitialized error covariance nor rpp sirpijigriiorro ;r 
oect ion Vj IS 



p(i- 



k.’"l 
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(3 kts)^ 0 0 
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0 0 0 
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A. irURODUCTlON 

Error ellipsoids are useful in visualizing the estimation error, v/itn 

them Vv'e can consider the true state value to lie within a certain region 

surrounding the estimate .. This uncertainy is expressed in the covariance 

of error miatrix P(k }k). The concept of the error ellipsoid is summarized. 

DEFINITION. Suppose the n -dimensional vector randcmi variable x has a 
multivariate gaussian distriDution with a mean value of zero and covar- 
iance E[x'x^]=P. The "error ellipsoids" are defined as n-dirnensionai sur- 
faces of constant proPabilitu densitu. [Ref. 6-p. 252] 



The probabiiitu density function of x has the multivariate oaussian 



f orrn 



fj'<) = (27T,r'*''^|det P!’^^^.exp[-l/2(x‘^'P'^'x)j 



( 4 -\ ) 



Frorn v/hich the surface of constant proPaDilitu density is described as 

_ .^2 - 






.2 . 



IS an arbitraru constant. The name "'error ellipsoid 



'"‘■p'p - * r* 



the surface of constant probability densitu. The surface is an eilipsoid, if 
P IS a nonneqative definite matrix. The ellipsoids have a sirnpie proP- 
abliistic interpretation. For a specified value of c the probability that x 
lies 'Within or on the eilipsoid is obtained by integrating the probability 
density function over the surface of the eilipsoid. Table 4-1 lists the 
probab i i 1 1 1 es for a fev/ values of n and c [Ref. 8-p. 4-49]. 

From the equations given in Table 5-1 we can assume that the initial 
state of the system, and the random noise processes w(k) and v(k) are 
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7aDi5 4-1. PROBASILITIES FOR ERROR ELLIPSES 





c 


n 


1 


2 


3 


1 


.683 


.955 


.997 


2 


.394 


.865 


.989 


3 


.200 


.739 


.971 



i Oi pJ^Y p iU 



Gaussian. If these Gaussian assumptions are satisfied then the 
are also Gaussiari^ 

!. The state, xCk.), since it is a linear function of 

2. The estimate, xfkjk), which is a linear combination of x(k) and vfkT 

3. The estimate error. x(k) = xk | k) - x(k) 

As indicated in Section ill. the state estimation error is zero mean with 
covariance of error Pfk | K). If P(k | k) is nonnegative definite the surface 
is an ellipsoid. 



B. APPLICATION 

The first and third components of the state vector feouation [2~\ii 
represent posiiion. the second and lourth cornponents '’eDreseni veioc'tu 
and the fifth corriDonent represent frecuenoj. A ellipsoid for the total 
matri.\ is a conalorneraie mess; so it is logical to e.-;amine a supmat- 
relatino the state variaDies of the position components or tne eiocitu 
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The diagonal terms Pi.i(k|k) and P33(k|k) of the error covariance matrix 
•••epresents the variances of the estimate in the x and y positions respect- 
fuiiy. The off diagonal term Pi3(k|K) represents the covariance of the 
estimate in x and y. This term describes the degree of coupling and the 
orientation of the uncertainty in the x-y plane. 

The mult ivariante Gaussian oistriDution reduces to a tuvariate Gaussian 
distnoution with a proDaDility density function given by 



i .uixy < 



= .Om.'" 1 1 



V]-r'2'rl/: 



exp 












y \ \ - t ^ \ 
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where the means of the random vanaDies x and y are. u 

the parameter r is called the correlation coefficient of the ran- 
dom variables x and y. 

From which a curve of constant probability is described by 






2 -r-x-y ^ 
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Tr.i$ cijr-'b. 'iS on ^1110.-;^. Tne term error elliDSOiG often refers lo tns 
specific case wnen c is set equal to one. From Taoie (^-i) for tne case 
when n=2 and c=1 tne probaDility tnat a sample point will De within the 
ellipsoid is .394 . The major and minor axis of this ellipse are not 
aligned with the coordinate system. Since the error estimate x(klk) is 
normally distriPuted, the coordinate system can be rotated in such a wray 

matrix A represent a rotation of the axes through an angle 0 

'cos8 sin0 



that in the new system position components are uncorrelated. ui 



A 



-sin0 COS0 
Bu apply ina a linear transformation 






u' 



COS0 

-sin0 



sin0 

COS0 





'' \ 
\ 1 
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u 









t' 1 

I * 



and picking the angle 8 so that 
tan 20 = 



x•r•Ov'a^ ^ 

2^ 



O w U y 



•covfx.y) 



var X -var y 



(4-8) 



hoprc^ 



0 = 1 • tan"' 


2-PI3(k 


k) 


2 




k) - 1 





1 • tan 



r1 



i-r-n -rj 



n ^ ~c, ^ 



(4-9j 



v/e oDtain uncorrelated random variables x' and u’ [Ref. 9'-p. -"'-'I 



f ^ - j . J I t c 



/anances in this system are calculated by- 



•2 - 2 , 



^ cov(x,y) 



3 in ^0 
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Si Pi 20 



In Of'<j6f to Q0t 3 & 0 tl 8 r’ und6PSt3nc]iriO 3n &A3rnDl0 vvili ii'9 G'v>rri. 



FiCury (4-!). tht t3Pu0t iS OH 3 CuuPSG Of 180 u9GS 3t 



'■) K lb. iiitr 



Sucu is DPOviding frequencu and Dearing measurements, yvniie the LOFAR 
Duoy iS D-roviding only frequency measurements. The error ellipsoids for 
this figure are twenty times their actual size. At time tu = lO min the 
DiFAR buoy's bearing measurement's position components submatrix of the 
error covariance matrix is 

' 2.39 X lO'^ 5.96 x 10^ 






5.96 X 10"^ 7,54 x 10"^ 



( 4 - 1 ^ 






H 9nCrr . 



Ov = 166 yds. 

Oy = 270 yds, and 

n r 1 OQ line 
•• XU ' -• -■ y • 

Substituting these numbers into e<quat!on i4-9.) gives 



0 = 1 • tan: 



-1 



2 ■ 5.96 X IC^ 



- ~ 2 ‘"' d*^CjS ( 4 ~i 5 



'■ 7 q V 10 *^ - 7 d V inn 

rroni ^ciiStiOnS 3nd (4”i \) in© nsvv' uncorr9!3i0G '/3ri3ifC©s 3r© 

'i 7 0 y ^ “7 74 '/ I OfA^ V ! 0*^ 0*^ V ^ ^ 

. - ••• • L-/ y , I f i ^ t w' O A I V I . w' * • * * i 4 



.2 - 



Sin 2 (-29 ) . 



and 

•n '2 
- u 



= 2.39 10“ » 7.34 x 10“ - 3.96 x 10“ 



9.55 X ] 0"^ uds^' 



cjn 9 r-9Q '! 



'.9 new standard deviations are 

o,.; - 44 yds, and 
c J '• 509 yds . 
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If these values are multiple Dy the scale factor, they agree with the 
values from Figure (4-1), Note the majority of the error is along the 
hearing line. 




i-igure 4-1. Error Ellipsoid Example 
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A [uC'ical trad for a 5 kt target iS snown in Figure (5-- 



I / . * ' • s 



target’s initial position, sc-seo ana course are 

.•1 ~ aocr a mate ~ lU nn'i v2C2*3.ii.7 ucs) 
g- coordinate - 12 nm (24304.4 yds’ 

^ r I ¥ 

CipCC J ~ _■ f. I. o 

course = 180 degs . 

Tfie solid line is toe positional time history of the target’s track. The 
small circles indicate the position every five minutes. The target 



algorithm and output is contained in Appendix A. TaDle (5-ij lists tne 
times that the various maneuvers 'Xcur in Figure (5-1). Other target 
algorithms were developed and tested, hut Figure (5-1.) pattern is used in 
all simulations presented in Section VI. 
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Figure 5-i Typical Track of the Target. 



41 



rti 



r f, 1 “ 



1 t*."- »- *■• 

t . > 1 «' " 


' 1 . 


1 r\ 


::* ‘J • J < u> • ‘. 


1 1 ” i V 


0 r. •♦■ ^, K -1 k' n •♦' 1 • 

r:* loi 'J ‘.oi M 


N-25 


sti'oigrii 


1 6- 3 7 


iOr'jS 350® stdi'DOord turn 


vO~0'U 


SUSiQi'it 


5 i ■ jC 


small 360® starDoara turn 


37-70 


siraioni 


1 

1 

i. I 


00® starDoard iu>'n 




Slf a'Crn. 




00® siai noai <1 




!;:>• HinnT 


92- 1 0 ■ 


sn'isU '"s" 


i 0 2 - ' 0 '*' 


'•ira^gni 


i ! i j- i 23' 


i^rnc • ij[ n 


1 , i 72 ^ 


sir3'0^‘i 


! 3 P“ ^ -^0 


90^ i-T.^rnriHfn 



- '^u> /c ‘ - I 






in* 



t'srger ? position ana iho sonoDuoy patto^i'. ""‘■o f'O'sy 
^roqijopiCy 3nd Dpsnnc rnsssurstTi^nts !t is C'"^nv9nipn^ [9 

rpiru' pT 03 /^h K'H 2S 3rT!V!n?Cl in 3 [>3rk 1 l^d r>hf'op. ,*3t 



"••• C:'^ r-r,r\r 74 i n ) HP 



T KPCj 



^■C^T 1 ^ I .^' 3 f ! pr; rr T ^ n o c r\r r,r, n p 1 1 c h | n'“» ■'"rn I 



^•1 ^Mf' Li r.‘ f'r*c ■•4W-I (K Gmonr tTroPMonr-M nr 

Jr • • ...... . . . ^ ^ 



■' "V 



" 1 ^ 



T.M*:r tTl^3Sur<rrr«*rMt , 300 

C 1 r* t <-1 H li^ ^ r^\ > y '-xt \ r-.tr- 

- . s i.c t i.«*ji iu3i ‘j ‘-j^r V I d ». ! 'ji ». 



I • t .“' 

j , I n I Itr 

"he tracK'.ng jigorithrn rnakes no assun'iDii 
reaulariiu oi tne measurement arrival times. Measurements are likelu : 



I ui I 



.•» .■*, »,• 

ji j’wn II 111*^ 



- — 1 ♦ f' • t J-’ • .-1 ^ r-p 

• A. v-Lii 1 i 2*1 I'J'JI } 1 


^ t r'.--N r**' 

t iiiit;r-. 


"T K r f- 1 fv-| ^ f' 

1 1 il i 


r 'jr Ir? i ; i 


:: 1 wL’J i .J II J ?. 1 'ji d. ir 


1 fV> » 1 i f 1 

c. 1 1 . 1 u ! V i 


1 P r- P 1 p *dt P P f I p. p 

1 2- ill r '-w » a' M 


^ • r f ^ p f-T 

/ i , .i ir 2 23; ^r 



‘ h..- 



^ i-x, ^ r t • r" r ^ t ^ I r K* < f rv*' ! r" i • ^ 

. I I'T dr 'j^ ^ - uk.. r.- II J Ui { r V r r »J I ( 1 1 1 iu L t: . 



C I , . . . 

iUU'JULf 



Ml 



i I J V .4 



Pattern 

he sonoDuoy oaitern .must De k.nown to the. aicorirnm at tne 
of the simulation. The sonoDuoy positions are assumed to reman 
throughout, the simulation. There are existing methods to est.mate 
sonoDuoy position drift. An -input data set provides the fol loving 
information 

!. Mum.Der of sonoDuoys in the pattern 
J. lype ci sonocuou (le DIFAR or LOFARx and 

:oordinate and y soordinate in nauti:?.! 



■ ir 3.v..‘i I'jL-'uLM-j j.vUrMv!uii v* 



M 

rniles). 

Pn exa.mpie of this data set is contained in Pr.rend:''- a. 

d* **.-. 10*1 T *T I r-. f 

I cmicmc 









binCr tne noise-frer target position jm. am:- .j*. 

p-'. s ! t iS dir KiiL'/vti, me iiOise-iree Dearino rriedsurenterita tari 

from eduation j- 1 2;. To calculate the noise-free frecuencu rneasur 
from the Doppler equation (2-1 1 ), the fol levying quantities must ce rno^vn: 



' ♦■V--, fc- •< 



43 



i Jj 



1 






' T jLci , { ,- , u ir Li u»r I Ju i l r J m r 

;ii i'*^i.rrf.'r Dodd ! t'-‘ 'ODut'f. ji'o cC'iTiD 3 rt''j. 300 



u 'J V I I'w Li / •,-« \-j « I ^ L • t * 



,-^i '• \ I \ ‘J it ihfr’ 5<u't-^’U U1 buUj i-J m’ j U , y p,, 

! Ou jfii Ufiri: O'Ot SuDl'm I' ir'j Du tHb UStri dl .dldTl '.m U' d m'Vmj d r. 

I Dr rfir d“Mjr rfrirni nCOSc IS dSSuiTlSQ to i'idvO 3 OduSSldTi Q 1 sir ! L'Ul ’ O m !l'" 

zof 0 rri0dn dno known vOfidncc'. wo dssun'io thdl tho diTiouril 'ji tho rnoos" 
ijrernorii noise dODenOs on the distance the target is from the sonoDuou. 
So the value of the measurement noise standard deviation. deDends on me 
fange which is oDtained from equation (2-1), Table (5-2) lists tw'O sets 
of tuDicai sianoard deviations used for the freouencu rneasureiTient noise. 

I Vf'O sets oi DearinQ measurement noise standard deviations are iisteo m 
T.dule i5-3.u 

i lie ii iC'L I Outiiies u/^uoS and OouBd ar c coinDiiied \o vr uviue i 
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^A'.'vor' '.rifi-A 01 hr'A'or rnoTt'ix cioo 

2. i t A 'jQSOt v'oT. iiji) pOCf 01. . 

Trie mijiripie sonoDuoy tracking algontnm is oivioeo inio [nroe soaoes. 
Trie nrst s[age is tne initialization of the Kalman filter. The ExrenoeG 
i^alman filter algontnrn is containeo in the second stage. Stages one, and 
two. are comDineo with the ODservation packet into one FORTRAN programi 
wnicn IS contained m -^pDendr>' 6. The last stage consists of tne filter s 
oraphica! untDijr. wnich is contained in Appenoiv C. 
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5-2 r''j!tip!9 SonoDuoy fraci'iog Algorithm, 
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-'f 5 ir u •' I lu I Tw'-iM rc. 

nis informa-ion is sssumo: •.: :o s. s;i- 
3 ule inroijgn nurnan operarors, received from oirier tracKir.g rouiines. or 
some external sensor. Since tnis algor itnm was design for 
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close range tracNing, me a priori information is assumied to ne accurate 
(i.e., position is Know within 2 nm. velocity is known within 5 kts. and 
course within 15 degs). To start the cyclic process the foilov;ing .tuan- 
tities are recui-'ed; 

1. Tne target's estimated position, speed, and course, 

2. Knowledge of tne accuracy (i.e., standard deviation; the apco'e 

t- 1 ^ t- 

H U sdJ L ’ll r. , 

5. The center frecuency, f,-, , 

/-"i I ec'tiiiiate us the c-ueeu oi c-uunu Ui tiie .•'»ai.e'i , -/v . jjiiu 

5. I ne sotioDuou pattern entered ifi terms of Duou tupe oi and uOSitiOi.. 

The initial estimated state vector x(0 I ■! ) is oDtaineo irorri i,., 
and the target s estirnateo position, speed, and course, Tne target s esti- 
mated position, speed, and course are defined as 
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the u coordinate initial estirnated Dosition in nm. 
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the accuracy of the target's estimated position, speed and course. The 
standard deviation of estimated Dosition. speed, course, ana freauencu are 
deftneo as 

Op = the standard deviation of the estimated position in nro, 

Ovo - the standard deviation of the estimated speed in kts. 

Ok,. = rne standard deviation of the estimated course in degs. arc 
- trie standard oevtation of tne '■reduencu >n nz 
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in orclGr' to uoin 3 D9ltor undorstonoiriQ of ths troctinu oluontnfTi 
recursive process the foi lowing examples will be discussed, 
a. Example i 

Consider the case of only one DIFAR Puou having coritact on the 
caroei, ihe aiQorithrn reads the first observation oacKet vyhich corisists 



u> 



i. Initial time. 



2. D i F /-,R Du Oil DOS i t i Of'. 



. i ne freouercu measurement, arid 



4, The bearing measurement. 

rroii'i tiic if'ii t la 1 izat iOi'i staoe we nave tne initial state estiinate V i 
and Its error covariance Piuj ~ij, Foiiovvino rigure’S and * j~i j tne 
sequence of calculations goes like this 

i. Evaluate h( 0) by appluing the frecuencu measurement ano A0| -1 / to 
eduat ion (5~5b). 

E. Compute the Kalman gain GfO) by applying Ptoj “1 i to eduation o“iyj 

j. Compute tne uPdateo error covariance for the freouencu measurement 
" r' 0 ! 0 j from eduat ion f i Gu 



'^-or.'iDu t e tne uDdateu state estirnate nar' tne treduencu ■'r!easui'en'ie''it 
’■■X 0 I U ; t rnm eouat ion •. i / ). 
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Since tiicre is also 3 Dearing f'neacurerrient for mis Duoy, tne uso?. 
s'3te esfirnate !ir(0|0j and error covariance Pf(0l0j Decorne fCe 
ii'iO'jrs to tne Dear ii'iG iTieasurernent estirriotes. nence, 

0 i “ 1 ) ■ Xf( 0 1 0 ), and 



r 'nlu ; “ I ; = fUi | U i 



.. I 



c'>>3iU<3[^ Hlui Dli oC'L.- ilpnu inP DHoTinQ niecSUrHiTiHf'it ciTiO 1 i [ 

‘^0u-3[ ion 1 5"5 d j. 



L f '*J ^ ^ ^ ^ 3tfTi3^*' Go 1^3 Du 3DD ! U i “ i f D3 ^GU-3M0"'* 

s - t P, I 



Tpi^ ‘^^*ror Dov3r»3nc9 fo^ tn^ D‘t3r^no ^^“^5sur 
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uor"'‘P'jl9 tri 9 up03i90 st3t9 9Sl:nri3t9 for ^09 D 93 r’nQ 

•V 11)10) T orn ^'“’5 ‘ ^ 



ine next siep is lo project aneaa, Dut in order to project aneac 
trie next time must be known. So, when the next observation 
packet comes available the algorithm computes xtl |0.) from 
equation (.3-4) and P(! |0) from equation (3-20). 

■rocess is repeated by re-cycling through steps l-iO. 
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sonoDuoy pattern, one DiF^.P Duoy <ca!lec C 
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continues with step 4 or 9, If the predicted residual is greate!' than the 
adaptive date repeat steps 1-7 above, it the adaptive date is exceeded 
three consecutive tirnes the error covanarrce rnatrix P(kjk-l) is reini- 
tiai'ced. The alooritrim continues the process startino with step 4 aPC've. 



c 




^ \ f '"T — - r* r-.i }^**r : p ■. 

— i r. ^ I 1‘.. . i 



- i 



C.-r.r of tr.9 ODjectives oi' tl'iis researcri v/aa to p^'OuUce a 0D9rai;-:r,a 
cornpuier program for passively trac^^ing a target from an airpcurne piat- 
lorrn. The purpose of this^section is to uemonstrate through scenarios me 
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demrnstrates the initialization process. 
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wnere tne values of , and Oj?^^ are given in equation (2-7-. 
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Tie intent of second random forcing function covariance mauTx is to 
produce a state excitation covariance matrix that has little effect on tne 
performance of tne filter. In this situation only the adaptive control 
metnoG or measurement noise is effecting the filter's performance. Let 
T.ne secono random, forcina function Q’:-(k) be defined as 
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3 iiTiu !at ions. Four noi3e~rree sirnulations are illustratecl in Fraures 
(6-5). In Figures (6-6) - (6-9) measurement noise from Table (5-2.) set 1 
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The target track is described in Subsection V.B . The dashed line is 
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position every five minutes (i.e., the first "x" is the target’s estimated 
position from the updated state estimate x(0!0), the next 
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Figure 6-2. Scenario i-Simulation ]■ 
Enlarged Geograpmc Plot- Q' 2 (k) Applied 
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Figure 6-3. Scenario 1 -Simulation 2- Enlarged Geographic Plot- 
Q’2(k:) and Adaptive Control Applied 
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Figure 6-4. Scenario 1 -Simulation 3^ 
Enlarged Geographic Plot- Q'|(k) Applied 
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Figure 6-6. Scenario 1 -Simulation 5-Enlarged Geographic Plot- 

Noise and Q'2(k) Applied 
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Figure 6-7. Scenario 1 -Simulation S^Eniarged Geographic Plot- 
Noise. Q' 20 ^'), and Adaptive Control Applied 
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Noise and Q'i(k) Applied 
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Figure 6-9. Scenario 1 -Simulation S^Enlarged Geographic Plot- 
Noise. Q'i(k), and Adaptive Control Applied. 
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D-?r;!n3 ^veru maneuver ano combleiely rnisaes the sm.ai! 360 oeeturn. 

O'l F’C'Ure tc-5.) me adaptive cor.moi rnethoG ana Q':'''' are amver. 
As can &e seen from Figure (6-3) and Table (6-2), the filter estimates are 
slightly off during the large 360 deg turn and off by as much as 394 yos 
in the small 360 deg turn. Table (6-3) lists the nuniber of times the 3-cr- 
adaDtive gate v/as exceeded for each measurement. Note that only the 
frecuency measurements cause the adaptive gate to be exceeded. 

In Figure (6-4) the random forcing function Q’,(k') is applied to the 
filter, in this simulation the filter estimates accurately reconstructs the 
track until the end of tne large 360 deg turn (about time tm 3-‘ rn:r,s;. 
The estimates lag slightly behind the actual track from tm3S mins t: tne 
end v/ith an average error of approximately 95 yds. 

Both Q',(k) and the adaptive control miethod are applied :: the 
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i 00 yds for j rfiins) dunng the large i6C dec turn. laDle (b-'^’ .ists '"e 
nurnber or times the adaptive gate v/as exceeded* tor eacr* rneasurerr.ei't. 
.caii'i onlti the freouency measurernent adaptive gate is exceeded, con'- 
paring Table (G-3) to Table (6-4). it can seen that by increasing the 
random forcing functhon oovarinace matrix, the adaptive gate is exceeded 
far less. As expected this simulation reconstructs the actual track better 
than the simulations 1-3. 
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TaDle 6-3. SCENARIO l -SinULTATIOM 2'- 
NUMBER OF TIMES THE ADAPTIVE GATE IS 
EXCEEDED FOR EACH MEASUREMENT 





T 5 r Z _ .-1 C r CM ' “ f 1 -NM T T I ‘ ’• • 

: Jlmc w.w! ’wL ^ *- j ]'..*• i ^• 

mviDC<“- r>c TTMCC TuC a rv « ni"r^ .-C ^ ^ tC IC 

iw'i S L L. V * I ♦ I L. w* : » iw L-' S j ^ L. • t« i 

c/rFrrpr> cr-,p pirp, 

^ • w- i_ t- L U. U* 1 ■W « ^ U • M « I ,_ . « w- w‘ • I W -I- j « 




' Noisu Simuiatjons 
As indicated in Table (G-l), measurement noise is applied to simu- 
laticns 5-8. Figures (6-6)-(6-9) respectively. From Table (5-2.) set 1 fre- 
duency measurement standard deviations and from Table (5-3) set 
bearing measurement standard deviations are applied to the 
measurements. Lire Table t6-2) for the noise-free simulations. 

^‘5-5j lists tne maximum position error for eacbi maneuver in Simulations 
Tne a rmrari information is tbe same as tOe noise-free sin'Mjiatiors. 
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In all four simuiations, the first estimate x(Xi|0j is 494 yds southv/est of 
the actual starting position. Examining the error ellipsoids gives us some 
insight to this error. The center of error ellipsoid is the measurement 's 
estimated position. The large dashed circle error ellipsoid is due t: 
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Tahle '6-5j to Sirnulanon i errors in Table l6-2j, it :an oe seer, that tr.e 
only major difference is in the first leg, time ti,=0-i0. ^s indicated 
above this difference is due to the noisy initial measurements. 

In Figure (6-7) the adaptive control is applied to the filter. The 
results of Figure (6-7) are sim.ilar to that of Figure (6-3), except for the 
small 36C deg turn. Table (6-6) lists the number of tiiTies the adaptive 
gate v/as exceeded for each measurement. 

■ Tne random, forcing function covariance matrix O'SK,,) and noise 's 
applied to the filter in Figure (6-Sj. The results are similar t: the Figu":! 
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exceeded the adaptive gate at that particular time. Figure (6-10) agrees 
with Table (6-7) information. We can see in Figure (6-10) that during the 
90 deg turn at time t|< = 12 and t|< = 13 the adaptive gate increases to 
admit the frequency measurement from buoy l to the filter. Similarly, 
the adaptive gate increases during the large 360 deg and small 360 deg 
turns. 
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Figure 6-10. Scenario 1 -Simulation 8- 
Frequericy Heasurement-Predicted Residual and Adaptive Gate 
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Figure 6-1 1. Scenario 1 -Simulation 8= 

Bearing neasurement-Predicted Residual and Adaptive Gate 



Figures (6-12) and (6-13) show the variances of the position 
components, x component is Pu(k|l<) and y component is P 33 (l<|k), from 
the error covariance matrix for the frequency and bearing measurement 
respectively. Both figures show the x and y component variances de- 
creasing rapidly. This is due to the a priori position information, Cp 



75 



equals 0.5 nm (1012 yds). Hence the variance is approximately 10^ 
yds^. Figures (6*M) and (6-15) are the result of expanding Figures (6-12) 
and (6-13). In these graphs we can see how the position variances change 
during each of the target's maneuvers. Note that the frequency and bear- 
ing measurement position variances are nearly the same for each buoy. 
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Figure 6-12. Scenario 1 -Simulation 8- 
Frequency Measurement-Position Variances 
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Figure 6-13. Scenario 1 -Simulation 8^ 
Bearing Measurement-Position Variances 
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Figure 6-H. Scenario 1 -Simulation 8* Expanded View 
of the Freguen‘:y Measurement Position Variances 
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Figure 6-15. Scenario 1 -Simulation 8^ Expanded View 
of the Bearing neasurement Position Variances 
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sisretj of a ten term moving window. From Helstrom, [Ref. 9 :p. 219 ] trx 
most convenient form for calculatinq tne sample variance is 
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increases for all tkiree Duous from ti, = 30 to ti, -35 mins because me u 
component error is greatest during this tirneCfrorn ti, - 32 to ti., =35 mins 
the error is greater than 100 yds). 

Figures ( 6 - 19 ) and ( 6 - 20 ) illustrates the change in the veiociiu 
variances, y^^- p22(kjk) and Vy- p44(kjk), for simulation 8 . Note that tne 
h'eouerKPu and bearinc rneasurernent velocitu variances are very similar, 
'"he /eiocn.ij variances of buou l increases durinc each of the I'rianeuvers. 
DuOu .'di iances are oniu siiGhtiu etiecieo Du me 'aO deo tuf n. out as me 
taroet iViOu'es tvj'w ard the Duou tne variances increase, especially •. .. uur~ 
ing the iarae 560 deg turn v',j has a Greater increase for duou 3 . This is 
er.Pecied due to DiFAR 3 s locatiori. 
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Figure 6-16 Scenario 1 -Simulation 8* Buoy I’s 
Position Variances and Experimental Variances 
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Figure 6-17. Scenario 1 -Simulation 8- Buoy 2's 
Position Variances and Experimental Variances 
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Figure 6-18. Scenario i-Simulation 8* Buoy 3's 
Position Variances and Experimental Variances 
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Figure 6-19. Scenario I -Simulation 8- 
Frequency Heasurement-Velocity Variances 
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Figure 6-20. Scenario I -Simulation 3- 
Bearing rieasurement-Velocity Variances 
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Tne frequenc.y variance for the frequency ana Dear mg measurements 
are illustrated in Figures (6-21) and (6-22). Like the velocity variances, 
the frequency varianc.e for the bearing measurement is very similar to the 
frequency variance for frequency measurement. 
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Figure 6-21 Scenario 1-Simulation 8- 
Frequency neasurement-Frequency Variances 
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Figure 6-22. Scenario 1 -Simulation 8^ 
Bearing Measurement-Frequency Variances 
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Figures (6-23)-(6-28) illustrates the position, velocity and fre- 
quency components Kalman gains for the frequency and bearing measure- 
ments, In Figures (6-23) and (6-24) the position gains decrease towards 
zero with small deviations for the turns. The velocity and frequency 
gains, Figures (6-25) - (6-28) are very erratic during the turns. 
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Figure 6-23. Scenario 1 -Simulation 8= 

Frequency Measurement - Kalman Gains for Position Components 



We can now explain wny tne bearing measurement's variances are 
similar to the frequency measurement's variances. Recall the error covar- 
iance update equation (3-19) from Table 3-1 is 
P(k|k) = (I - G(k)-H(k)) • P(k|k-I) 

and that in this case P(k|k-I) is really the frequency measurement's 
covariance of error matrix. In Subsection V.D.2 it was shown that 
Pb(k|K-l) = Pf(k|k). 
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Figure 6-24, Scenario 1 -Simulation 8= 

Bearing Measurement - Kalman Gains for Position Components 
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From equations (3-46D), (3-460), and (3-46e), the partial derivatives of 
the bearing measurement with respect to the velocity and frequency 
components are zero. Hence, only the position components of H(k) are used 
in the calculation of G(k)'H(k). It can be seen in figures (6-23)-('6-28), 
that the Kalman gains are not always small numbers. But, examining the 
output data indicates that the position components of H(k) are very small 
compared to G(k). so G(k)'H(k) is be small. Therefore the error covariance 
matrix for the bearing measurement P(k|k) will be approximately equal to 
the error covariance of the frequency measurement P(k|k-1). 
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Figure 6-25. Scenario I -Simulation 8- 
Frequency Measurement - Kalman Gains for Velocity Components 
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Figure 6-26. Scenario I -Simulation 8^ 

Bearing Measurement - Kalman Gains for Velocity Components 
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Figure 6-27. Scenario 1 -Simulation 8- 
Frequency Measurement - Kalman Gam for Frequency Component 
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Figure 6-28. Scenario 1 -Simulation 8*- 
Bearing Measurement - Kalman Gain for Frequency Component 
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C. SCENARIO 2 



Scenario 2 is a continuation of Scenario 1 -Simulation 8. The last 
state estimate ^(59|59) and last error covariance matrix P(59|59) from 
Simulation 8 is used to initialize Scenario 2. DIFAR 3 from Scenario I is 
dropped and another DIFAR 3 is placed west of the target's track as illus- 
trated in figure (6-29). An enlarged geographic plot is shown in Figure 
(6-30). Table (6-8) lists the maximum position error for each maneuver. 
The left hand column of Table (6-30) is taken from Table (5-1). The 
filter’s estimated track accurately reconstructs the actual track. As 
shown in figure (6-30) and Table (6-8) the maximum errors occur during 
the last segment of both "s* turns. 
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Figure 6-29. Scenario 2- GeographicPIot- 
Noise, Q’i(k), and Adaptive Control Applied 
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Figure 6-30 Scenario 2- Enlarged Geographic Plot- 
Noise. Q'i(k), and Adaptive Control Applied 
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Figures (.6-31) and (6 
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lustrates tns predictea residual and 



adaptive gate tor the frequency and hearing measurements respectively. 
Note that adaptive gate for Buoy 1 in Figure (6-31) is exceeded two times 
at t', = iOO mins. Lire Scenario 1, the bearing measurem.enfs aaattive 
adte IS riever exceeded. 
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Figure 6-31. Scenario 2- Frequency Measurement- 
Predicted Residual and Adaptive Gate 
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Figure 6-32. Scenario 2- Bearing Measurement- 
Predicted Residual and Adaptive Gate 



96 



Figure (6-33). The values of the bearing measurement’s positions 
variances are slightly less than the frequency measurement's position 
variances values. 

Figure (6-35) illustrates the velocity components variances for the 
frequency and bearing measurement. As indicated for Scenario 1. the 
bearing measurement's velocity variances are very similar to the fre- 
quency measurement’s velocity variances. Hence, only one graph is dis- 
played. Note the large value for Vy variance for buoy I at t|< = 100 mins. 

This is when the target is completing the small "s" maneuver. 
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Figure 6-33. Scenario 2* Frequency Measurement- 
Position Variances 
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Figure 6-34. Scenario 2- Bearing Measurement- 
Position Variances 
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Figure 6-35. Scenario 2= Frequency and Bearing 
Measurement - Velocity Variances 
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The frequency component variance is shown in Figure (6-36). Since 
the bearing and frequency measurement's frequency component variance is 
nearly the same, only .one graph is shown. 
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Figure 6-36. Scenario 2 - Frequency and Bearing 
Measurement-Frequency Variance 
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Figures (6-37)-(6-42) illustrates the Kalman gains for the position, 
velocity and frequecny components. The position gains. Figures (6-37) and 
(6-38), are usually less than i 100, but there are deviations when the 
target maneuvers. Again the velocity and frequency gains. Figures (6-39)- 
(6-42), are very erratic during the maneuvers. 
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Figure 6-37. Scenario 2 '- Frequency (leasurement- 
Kalman Gains for Position Components 
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Figure 6-58. Scenario 2' Bearing Measurement- 
Kalman Gains for Position Components 
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Figure 6-59 Scenario 2- Frequency Measurement- 
Kalman Gains for Velocity Components 
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Figure 6-40. Scenario 2- Bearing Measurement - 
Kalman Gains for Velocity Components 
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Figure 6-41. Scenario 2* Frequency neasurement- 
Kalman Gain for Frequency Component 
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Figure 6-42. Scenario 2-= Bearing Measurement - 
Kalman Gain for Frequency Component 
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D. SCENARIOS 

Scenario 3 is a three sonoDuoy scenario, where two of the buoys are 
LOFAR. A geographic plot of the target's track, sonobuoy pattern and the 
filter's estimated track are shown in Figure (6-43). An enlarged geo- 
graphic plot is shovyn in Figure (6-44). The target's track is the same 
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Figure 6-43. Scenario 3- Geographic Plot- 
Noise. Q*,(k). and Adaptive Control Applied 
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as the traci< in Scenario 1 and is described in Subsection v.B . The 
algorithm generates the estimates and predictions from the measurements 
in the following order* 

1. Frequency measurement from DIFAR 1 

2. Bearing measurement from DIFAR 1 

3. Frequency measurement from LOFAR 2 

4. Frequency measurement from LOFAR 3 
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Figure 6-44 Scenario 3= Enlarged Geographic Plot- 
Noise. Q'i(k), and Adaptive Control Applied 
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r;o !. The rsndorri i'orcir.g function covariance I'nairix G')(k), the acactive 

gate, ano noise are applied to the filter. From TaDle (5-2) set 2 frequency 

measurement standard deviations and from TaDle (5-3) set 2 Dearing 
measurement standard deviations are applied to the noise-free measure- 
ments. The measurement noise standard deviations used in Scenario 3 are 
two times the measurement noise standard deviations used in Scenario i. 
TaDle (6-9) lists the maximum position error for each maneuver, ^re 
errors are expected to De larger Decause twice the measurement noise nas 
Deen asclied to the simulation and there is only one Deanng measurement 
per time interwai. 
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Ccar'r.G n^3z\j'r^rc\^r>i. This noisij oearing rri^rasurameni trorr, D!F-R : la 
' .?j ^aaa. M U 0 G? r.*ior 0 tnan me noise-irae rncasurernerit. inis Gauaas tna 
aai’n-iaieG DOsUion to De nlO gds south of the target's actual startioc 
GGSiticn. LOFAR 2 irecuencu rneasurernent esiirnateG Gosiuor. is heanu 
the same as DIFaR rs DeanPiC measurement. LOFar : noisy irecuenc. 
mieasurement is 0.13^ iosver than the noise-free GODpler ire.tuercg 
measurement, its e.stimateo Drjsition is 808 yds southwest of the act -a. 
target. LOF..AR 3 shifts the erro-r eiiipsoia to the ieft. hence, tne large 
initial error is due to DIF.AR I’s noisy hearing measurement ana LCRAR 3's 
noisy frequency mieasurem.ent. The shape of the error ellipsoiG inatcates 
that the majority of the error is along the bearing measurement of DIFAR 
i. The error ellipsoids for t|, = 10 m.ins and = 30 mins are align with 
DIFAR I's bearing measurement to the actual position, but the actual posi- 
tion IS outside of the error ellipsoids. The filter talces approximately 
tvsenty minutes to ioct on to the target with errors less than 250 yds. As 
can be seen in figure to-A-^) the filter detects maneuvers and tr.acrs the 
target thrcugn all the maneuvers. 

Figures (.5-A5) illustrates the predicted residual anc aoapti-.e gate ^‘cr 
the ’re.iuencu ana beavinc measurements. Note at tj^ = di m.ns. ".no 
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Figure 6-45. Scenario 3- Frequency and Bearing 
neasurement- Predicted Residual and Adaptive Gate 
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Figure 6--^6. Scenario S'- Frequen<cij and Bearing 
neasurement- Position variances 
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Figure 6-^7. Scenario 3^ Frequency and Bearing 
neasurement - Velocity Variaras 
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Figure 6--18. Scenario 3= Frequency and Bearing 
Measurerr,ent-Frequency Variance 
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Figure 6-49. Scenario 3^ Frequency and Bearing 
neasurement-Kalman Gains for Position Components 
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Figure 6-51. Scenario 3= Frequency and Bearing 
Measurement - Kalman Gain for Frequency Component 
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E. SCENARIO 4 

Scenario 4 is a continuation of Scenario 3. Like Scenario 2. tne last 
state estimate, ;i(59|59), and last error covariance matrix, P(59|59), 
from Scenario 3 is used to initialize Scenario 4. As illustrated if Figure 
(6-52), LOFAR 3 from Scenario 3 is dropped and anotner lOFAR 3 is placed 
west of the target’s track. An enlarged geographic plot is shown in Figure 
(6-53). TaDle (6-10) lists the maximum position error for each segment 
of the targets track. 
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Figure 6-52. Scenario 4: Geographic Plot- 
Noise, Q'i(k), and Adaptive Control Applied 
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Figure 6-53 Scenario *4: Enlarged Geographic Plot- 
Noise. Q'i(k). and Adaptive Controi Applied 
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adaptive gate. Figure (6'54) illustrates the predicted residual and 
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uuoy 1 causes tne preoicied residual to exceed tne adaptive gate three 
times and tne filter is reinitialized. DIFAR i 's noisy Dearing measurement 
for t^ = 10! mins is 15 degs greater than the noise-free measurement. 
The distance froni the target’s actual position to Duoy l is 3250 yds. A 
15 deg error at this distance is a very large error. After restarting, the 
filter Itxks on to the target at t|, = 108 mins with an error of 80 yds. At 
tk =11 1 mins the Dearing measurement is 13 degs less than the noise-free 
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Figure 6-5d. Scenario Frequexy and Bearing 
measurement- Predicted Residual and Adaptive Gate 
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position Dy 770 yas. The filter reinitializes again at t^ = 113 mins. 
DIFAR 1‘s noisy hearing measurement is 10 degs greater than the noise- 
free measurement and the target is less than 2000 yds from buoy 1. 
After restarting the filter continues to track the target. 

Figure (6-55) illustrates the position variances. Note the two peaks 
at tk =101 mins and t^ = 113 mins, this is when the filter is reinitialized. 
From equation (3-57) the position components are reinitialize to (0.5 nm)^ 
or .approximately 10^ yds^ . 
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Tne velocity var larces are shown in Figure (6-56). The variance for 
Duoy ! at t;, = 100 15 *4.32 x 10^ (ycs/mins)- . This is at the completion 
of the small "s" maneuver. The velocity components are reinitialized to 
(3kts)- or approximately 10*^ (yds/mins)'^ at ti, =101 mins and t^ = 113 
mins. Figure (6-57) illustrates the frequency variances. Instead of reini- 
tializing the frequency component to 1 hz^ as indicated in equation (3-57) 
the algorithm used 10 hz- for this scenario. The Kalman gains for the 
position, velocity, and frequency components are shown in Figures (6-58) - 
(6-60), 







Figure 6-56. Scenario 4= Frequency and Bearing 
Measurement - Velocity Variances 
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Figure 6-57. Scenario 4: Frequericy anc Bearing 
Measurement -Frequency Component 
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Figure 6-38. Scenario 4= Frequency anc Bearing 
Measurement-Kalman Gams for Position Components 
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Figure 6-59. Scenario Frequency and Bearing 
neasurement-Kalman Gains for Velocity Components 
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Figure 6-60. Scenario Frequency and Bearing 
neasurement- K.alman Gain for Frequerhcy Component 
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Figure 6-d1. Scenario 5-Simulation \- Geographic Plot- 
Noise. Q'i(k). and Adaptive Control Applied 
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Figure 6-62 Scenario 5-Simulation 2 ' Geographic Plot- 
Noise. Q’)(k), and Adaptive Control Applied 
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C XX* PURPOSE XXX 

C PROGRAM COMPUTES THE TARGET TRACK 

C xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 
C THE OUTPUT IS IN FILE DEVICE 3 
C xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx 

c 

c XXX VARIABLE DEFINITIONS xxx 



c 


DT 


= 


SAMPLE* INTERVAL IN SECS. 






c 


HDG 


= 


MATRIX OF TARGET'S HEADINGS 






c 


I 


= 


COUNTER 






c 


II 


s 


I-l 






c 


ITIME 


= 


INTERVAL TO TIME TURNS ITIME *12, 






c 






EX. ITIME X DT X TRATE * DEGREES OF 


TURN 


c 






12 X 15 SEC X 0.5 DEG/SEC * 90 


DEGS 


c 


J 


s 


COUNTER 






c 


PI180 


= 


PI X 180 DEGREES 






c 


T 




TIME USED IN SECONDS AND CONVERTED 


TO 


MINUTES FOR 


c 






OUTPUT 






c 


TACC 


= 


HORIZ ACCELERATION- YARDS/SECXX2 






c 


TRATE 


= 


TURN RATE-INPUT IN DEG/SEC, CONVERTED 


TO RAD/SEC 


c 






+ CLOCKWISE,- COUNTER CLOCKWISE 






c 


TURN 


= 


SUBROUTINE TO CALCULATE TURNS 






c 


VEL 


= 


MATRIX OF TARGET’S VELOCITIES-KNOTS 


AND YARDS/SEC 


c 


VX 


= 


X COMPONENT OF VEL 






c 


VY 


= 


Y COMPONENT OF VEL 






c 


X 


= 


X COMPONENT-USED IN NM AND YDS 






c 


Y 




Y COMPONENT-USED IN NM AND YDS 







C 

C xxx VARIABLE DECLARATIONS xxx 
C 
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REAL HDG 

DIMENSION T(2000) ,VEL (2000), TRATE( 2000) ,X(2000),Y( 2000) ,HDG( 2000) 
C 

C INITIALIZE TARGET DATA 
PI180=0.017A533 
X(l)=10. 

Y(l)=12. 

VEL(1)=5.0 

HDG(1)=180.0 

TRATE(1)=0.5 

DT=15. 

TACC=2. 

T(1)=0. 

ITIME=15 

C CONVERT DIST TO YARDS 

X(1)=X(1)*2025.3667 
Y(1)=Y(1)*2025.3667 

C CONVERT TURN RATE FROM DEG/SEC TO RAD/SEC 
TRATE(1)=TRATE(1)KPI180 
C CONVERT VEL FROM KTS TO YARDS/SEC 
VEL(1)=VEL(1)»0. 562605 
C CONVERT HEADING TO RAD/SEC 

HDG(1)=HDG(1)XPI180 

C CONVERT HORIZ ACC FROM YARDS/SECKK2 TO YARDS/MINK»2 
C TDATA(8)=TDATA(8)K3600.0 

C WRITE(6,1000) T,X,Y,VEL,HDG 

C WRITE(3,1000) T,X,Y,VEL,HDG 

1 = 1 

34 IF(T(I) .EQ.600. ) THEN 

C ITIME=12 90 DEG AT 0.5 DEG/SEC 
C ITIME=24 180 DEG AT 0.5 DEG/SEC 

C ITIME=48 360 DEG AT 0.5 DEG/SEC 

ITIME=12 

C 90 DEG -STARBOARD TURN 

CALL TURN(X,Y,TRATE,VEL,HDG,DT,ITIME,T,I) 



139 



GO TO 800 

ELSEIF(Td) .EQ.1500. ) THEN 
C LARGE 360 DEG STARBOARD TURN 
ITIME=A8 

CALL TURN(X,Y,TRATE,VEL,HDG,DT,ITIME,T,I) 

GO TO 800 

ELSEIF(T(I).EQ.3000.) THEN 
C SMALL 360 DEG STARBOARD TURN 
ITIME=2A 

TRATE(I)=1XPI180 

CALL TURN(X,Y,TRATE,VEL,HDG,DT,ITIME,T»I) 

GO TO 800 

ELSEIF(T(I).EQ.A200.) THEN 
C 90 DEG STARBOARD TURN 

ITIME=12 

TRATE(I)=0.5XPI180 

CALL TURN(X,Y,TRATE,VEL,HDG,DT,ITIME,T,I) 

GO TO 800 

ELSEIF(T(I).EQ.50A0.) THEN 
C 90 DEG STARBOARD TURN 

ITIME=12 

TRATE(I)=0.5»PI180 

CALL TURN(X,Y,TRATE,VEL,HDG,DT,ITIME,T,I) 

GO TO 800 

ELSEIF(T(I).EQ.5460.) THEN 

C 90 DEG STARBOARD TURN, BEGINNING OF SMALL S TURN 
ITIME=6 

TRATE(I)=-1XPI180 

CALL TURN(X,Y,TRATE,VEL,HDG,DT,ITIME,T,I) 

GO TO 800 

ELSEIF(T(I).EQ.5550. ) THEN 
C 180 DEG STARBOARD TURN 
ITIME=12 

TRATE(I)=1XPI180 

CALL TURN(X,Y,TRATE,VEL,HDG,DT,ITIME,T,I) 
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GO TO 800 

ELSEIF(Td) .EQ.5760. ) THEN 
C 180 DEG PORT TURN 
ITIME=12 

TRATE(I)=-1*PI180 

CALL TURN(X,Y,TRATE,VEL,HDG,DT,ITIME,T,I) 

GO TO 800 

ELSEIF(T(I).EQ.59<iO.) THEN 
C 90 DEG STARBOARD TURN, ENDING OF SMALL S TURN 
ITIME=6 

TRATE(I)=1.0*PI180 

CALL TURN(X,Y,TRATE,VEL,HDG,DT,ITIME,T,I) 

GO TO 800 

ELSEIF(T(I).EQ.65‘iO.) THEN 

C 90 DEG STARBOARD TURN, BEGINNING OF LARGE S TURN 
ITIME=12 

TRATE(I)=0.5*PI180 

CALL TURN(X,Y,TRATE,VEL,HDG,DT,ITIME,T,I) 

GO TO 800 

ELSEIFCTCI) .EQ.6720.) THEN 
C 180 DEG PORT TURN 
ITIME=29 

TRATE(I)=-0.5*PI180 

CALL TURN(X,Y,TRATE,VEL,HDG,DT,ITIME,T,I) 

GO TO 800 

ELSEIFCTCI). EQ. 7190.) THEN 
C 180 DEG STARBOARD TURN 

ITIME=29 

TRATECI)=0 .5*PI180 

CALL TURNCX,Y,TRATE,VEL,HDG,DT, ITIME,T,I) 

GO TO 800 

ELSEIFCTCI) .EQ. 7500. ) THEN 
C 90 DEG PORT TURN 

ITIME=12 

TRATECI)=-0.5*PI180 
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CALL TURN(X,Y,TRATE,VEL,HDG,DT,ITIME,T,I) 

GO TO 800 

ELSEIF(Td) .EQ.8250. ) THEN 
C 90 DEG STARBOARD TURN 
ITIME=12 

TRATE(I)=0.5*PI180 

CALL TURN(X,Y,TRATE,VEL,HDG,DT,ITIME,T/I) 

GO TO 800 
ENDIF 

TRATE(I + 1)=TRATE(I). 

HDG(I+1)=HDG(I) 

VEL(I+1)=VEL(I) 

VX=VEL(I)*SIN(HDG(I+1)) 

VY=VEL(I)*COS(HDG(I+D) 

X(I+1)=X(I)+VX»DT 
Y(I+1)=Y(I)+VY»DT 
T(I+1)*T(I)+DT 
800 1=1+1 

IF(I.LT.56<i) GO TO 3A 
11 = 1-1 

DO 20 J=1,II,4 

C CONVERT HEADING FROM RADS TO DEGS, 
HDG(J)=HDG(J)/PI180 

C CONVERT VELOCITY FROM YARDS/SEC TO KTS 

VEL(J)=VEL(J)/0.5626 
C CONVERT X AND Y TO NM 

X(J)=X(J)/2025.3667 
Y(J)=Y(J)/2025.3667 
T(J)=T(J)/60. 

WRITE(6,1500) T(J),X(J),Y(J),VEL(J),HDG(J) 
WRITE(3,1500) T(J),X(J),Y(J),VEL(J),HDG(J) 

20 CONTINUE 

1000 FORMAT ( F8 . 2, 2( 4X, FI 1 . 4 ) , 4X, F7 . 2, 4X, F7 . 2) 

100 FORMAT (F10.5,2X,F8.2) 

1500 FORMAT ( F8 . 2, 4X, FI 1 . 4, 4X, FI 1 . 4, 4X, FI 1 . 4, 4X, F7 . 2) 
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200 FORMAT (/,2X,»TIME SLOT = ’,IA) 

2000 FORMAT (/ , 5X, F5 . 2, 5X, FI 1 . 5 , 5X, FI 1 . 5 ) 

STOP 

END 

C 

C 

C 

C 

SUBROUTINE TURNCX, Y, TRATE, VEL , HDG, DT, ITIME, T, I) 
C 

C *** PURPOSE 

C CALCULATE X AND Y COMPONENTS IN TURNS 

C 

C VARIABLE DEFINITIONS *** 

C 



c 


DT 


= 


SAMPLE INTERVAL IN SECS. 






c 


HDG 


s 


MATRIX OF TARGET»S HEADINGS 






c 


I 


= 


COUNTER 






c 


ISTOP 


= 


TIME INTERVAL TURNSTOPS 






c 


ITIME 


s 


INTERVAL TO TIME TURNS, ITIME =12, 






c 






EX. ITIME X DT X TRATE = DEGREES OF 


TURN 


c 






12 X 15 SEC X 0.5 DEG/SEC = 90 


DEGS 


c 


J 


s 


COUNTER 






c 


T 


= 


TIME USED IN SECONDS AND CONVERTED TO 


MINUTES FOR 


c 






OUTPUT 






c 


TRATE 


= 


TURN RATE-INPUT IN DEG/SEC, CONVERTED 


TO RAD/SEC 


c 






+ CLOCKWISE,- COUNTER CLOCKWISE 






c 


TWOPI 


= 


2 X PI 






c 


VEL 


= 


MATRIX OF TARGET'S VELOCITIES-KNOTS 


AND YARDS/SEC 


c 


VX 


= 


X COMPONENT OF VEL 






c 


VY 


= 


Y COMPONENT OF VEL 






c 


X 


= 


X COMPONENT-USED IN NM AND YDS 






c 


Y 


= 


Y COMPONENT-USED IN NM AND YDS 







C 

C »»» VARIABLE DECLARATIONS 



c 



REAL HDG 

DIMENSION T(2000),VEL(2000) 

DIMENSION TRATE(2000),X(2000),Y(2000),HDG(2000) 

TWOPI=6. 2831853 

ISTOP*I+ITIME-l 

DO 10 JsI,ISTOP 

TRATE(J+1)*TRATE(J) 

HDG(J+1)=HDG(J)+TRATE(J)KDT 

IF(HDG(J+1) .GT.TWOPI) HDG( J+l)=HDG(J+l)-TWOPI 
VEL(J+1)=VEL(J) 

VX=VEL(J)XSIN(HDG(J+D) 

VY*VEL(J)3<COS(HDG(J+l)) 

X(J+1)=X(J)+VXXDT 
Y(J+l)sY(J)+VYXDT 
T(J+l)sT(J)+DT 
10 CONTINUE 
I^ISTOP 
RETURN 
END 
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output from 


target tracks 


input to 


tracking algorithm 


• 


file device 


3 








time 


X comp 


y comp 


velocity 


heading 


mins 


nm 


nm 


kts 


degs 


0.00 


10.0000 


12.0000 


5.0000 


180 . 00 . 


1.00 


10.0000 


11.9167 


5.0000 


180.00 


2.00 


10.0000 


11.8333 


5.0000 


180.00 


3.00 


10.0000 


11.7500 


5.0000 


180.00 


4.00 


10.0000 


11.6667 


5.0000 


180.00 


5.00 


10.0000 


11.5833 


5.0000 


180.00 


6.00 


10.0000 


11.5000 


5.0000 


180.00 


7.00 


10.0000 


11.4167 


5.0000 


180.00 


8.00 


10.0000 


11.3333 


5.0000 


180.00 


9.00 


10.0000 


11.2500 


5.0000 


180.00 


10.00 


10.0000 


11.1667 


5.0000 


180.00 


11.00 


9.9735 


11.0886 


5.0000 


210.00 


12.00 


9.9115 


11.0342 


5.0000 


240.00 


13.00 


9.8306 


11.0181 


5.0000 


270.00 


14.00 


9.7473 


11.0181 


5.0000 


270.00 


15.00 


9.6640 


11.0181 


5.0000 


270.00 


16.00 


9.5806 


11.0181 


5.0000 


270.00 


17.00 


9.4973 


11.0181 


5.0000 


270.00 


18.00 


9.4140 


11.0181 


5.0000 


270.00 


19.00 


9.3306 


11.0181 


5.0000 


270.00 


20.00 


9.2473 


11.0181 


5.0000 


270.00 


21.00 


9.1640 


11.0181 


5.0000 


270.00 


22.00 


9.0806 


11.0181 


5.0000 


270.00 


23.00 


8.9973 


11.0181 


5.0000 


270.00 


24.00 


8.9140 


11.0181 


5.0000 


270.00 


25.00 


8.8306 


11.0181 


5.0000 


270.00 
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c sampltt sonobuoy pattern input file 

c for time t=0 to t= 59 

c this pattern was used in scenario 3 

c a priori information used for p(k/k-l) and x(k/k-l) 

c 

c input data follows) 
c number of buoys in pattern 



buoy 


bflag 


X comp 


y comp 


type 




in nm 


in nm 


3 


DIFAR 


1.0 


9.0 


12.0 


LOFAR 


2.0 


8.0 


10.0 


LOFAR 


2.0 


12.0 


11.0 



c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 



sample sonobuoy pattern input file 
for time t=60 to t=1^0 

using x(k/k) and P(k/k) from previous run 

pattern and data is similar to that used in scenario 4 

input data follows) 

number of buoys in pattern 
buoy bflag x comp y comp 

type in nm in nm 

time 

p(k/k) = p(59/59) is a 5 X 5 matrix 
x(k/k) = x(59/59) is a 1 x 5 matrix 
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3 

DIFAR 1.0 
LOFAR 2.0 
LOFAR 2.0 
5.9000E-»-01 
1.79<*2E+0<i 
2.0082E-t-03 
3.9968E+02 
3.8682E-t-01 
-5.9835E+00 
1.5318E+0«i 



9.0 

8.0 
5.5 

2.0082E+03 

6.9507E+02 

-3.2991E+02 

-1.2371E+02 

-1.7275E+00. 

-1.4382E+02 



12.0 

10.0 

11.5 

3.9967E+02 

-3.2991E+02 

7.5759E+03 

<i.3116E+02 

7.8573E-01 

2.21<»2E+0<i 



3.8684E-i'01 

-1.2371E+02 

<».3116E+02 

1.5582E+02 

3.1255E-01 

-3.6561E+00 
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5.9835E+00 

1.7275E+00 

7.8573E-01 

3.1255E-01 

<i.8693E-03 

2.9992E+02 
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c exec's used to run the tracking algorithm 
c 

c example of temp exec used to reserve space on disk b 

c as indicated in buoy exec file device 7»8 10»12^18^ and 19 

c are on disk b. in Order to see disk b type "flist X X b" 
c 

c temp exec 

CP DEFINE T3350 AS 193 20 
FORMAT 193 B 
c 

c example of buoy exec 

c filedef 2 is sonobouy pattern input file 

c filedef 3 is the target track input file 

c filedef 4 is the graphic input for geographic and enlarged 
c geographic plots 

c filedef 7 is the graphic input for the kalman gains plots 

c filedef 8 is the output of the tracking algorithm. Each 

c matrices and calculation can be checked if desired, 
c filedef 9 is the graphic input for the variance plots 
c filedef 10 is the track position data and the filter's estimated 
c position output (in yards) 

c filedef 12 is the graphic input for the kalman position 

c variances and the experimental position variances, 

c filedef 18 is the graphic input for the predicted residual 

c and adaptive gating plots 
c filedef 19 is the error ellipsoid data 
c 

8TRACE ON 

CP TERMINAL LINES 80 
FILEDEF 02 DISK BU0Y2 DATA A 
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FILEDEF 


03 


DISK 


TGT INPUT A 




FILEDEF 


OA 


DISK 


FILE 


FTOAFOOl 


(PERM 


FILEDEF 


07 


DISK 


FILE 


FT07F001 


B 


FILEDEF 


08 


DISK 


OUTPUT DATA B 




FILEDEF 


09 


DISK 


FILE 


FT09F001 


(PERM 


FILEDEF 


10 


DISK 


TRACK 


DATA B 




FILEDEF 


12 


DISK 


FILE 


FT12F001 


B 


FILEDEF 


18 


DISK 


FILE 


FT18F001 


B 


FILEDEF 


19 


DISK 


ELLIP 


DATA B 




FILEDEF 


06 


TERH 








LOAD BUOY 


(START 
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c *** PURPOSE *** 

C THIS IS A MULTISENSOR TRACKING PROGRAM. PROGRAM USES EXTENDED 

C KALMAN FILTER TECHNIQUES TO TRACK A MANEUVERING TARGET BASED 

C ON NOISY PASSIVE BEARING AND DOPPLER SHIFTED FREQUENCY 

C MEASUREMENTS. 

C 

C IN ORDER TO RUN PROGRAM SEE THE TWO EXEC FILES DEFINE 

C ON THE PREVIOUS PAGES. TO RUN THE PROGRAM USE THE 

C FOLLOWING STEPS. 

C 1. RESERVE SPACE ON B DISK BY USING TEMP EXEC. 

C 2. COMPILE THIS PROGRAM. 

C 3. EXECUTE BUOY EXEC. 

C 

C XXX VARIABLE DEFINITIONS XXX 



c 


AFLAG 


= 


ADAPTIVE GATING FLAG; 0 . O-OFF, 1 . O-ON . 


c 


BEAR 


s 


SUBROUTINE TO CALC BEARING MEASUREMENT 


c 


BFLAG 


s 


DEFINES THE BUOY TYPE; 1.0- DIFAR, 2.0-LOFAR 


c 


BMEAS 


s 


SUBROUTINE TO CALC MEASUREMENT MATRIX H(K) 


c 






FOR THE BEARING MEASUREMENT 


c 


BRG 


= 


BEARING IN RADS 


c 


BRKKMl 


= 


PREDICTED BEARING MEAS. IN RADS, BRG(K/K-1) 


c 


BTYPE 


= 


BUOY TYPE, DIFAR OR LOFAR 


c 


D 


s 


DISTANCE OR RANGE, USED IN NM AND YARDS 


c 


DBKKMl 


s 


PREDICTED BEARING MEAS IN DECS, DBRG(K/K-1) 


c 


DBRG 


= 


BEARING IN DEGS 


c 


DOPP 


= 


SUBROUTINE TO CALC DOPPLER FREQUENCY 


c 


DSEED 


s 


NUMBER USED IN PSEUDO RANDOM NUMBER GENERATOR 


c 


DT 


= 


TIME DIFFERENCE, T(K)- T(K-l) 


c 


E 


= 


PREDICTED RESIDUAL 


c 


EXT 


= 


ESTIMATED X COMP, XKK(l) 
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c 


EYT 


= 


ESTIMATED Y COMP, XKKC3) 


c 


FD 


s 


ARRAY OF DOPP FREQS, ONE FOR EACH FREQ MEAS 


c 


FDKKMl 


= 


PREDICTED FREQ RESIDUAL FD(K/K-1) 


c 


FDOPP 


= 


THE DOPP FREQ, OUTPUT OF DOPP SUBROUTINE 


c 


FLAG 


= 


INDICATES MORE MEASUREMENTS IN THE TIME INTERVAL 


c 






1.0-REMAIN IN SAME TIME SLOT, 0.0-CAN READ NEXT 


c 






TIME 


c 


FMEAS 


s 


SUBROUTINE TO CALC MEASUREMENT MATRIX H(K) 


c 






FOR FREQ MEASUREMENT. 


c 


G 


= 


KALMAN GAINS 


c 


GAIN 


= 


SUBROUTINE TO CALC KALMAN EQUATIONS 


c 


GAM 


s 


GAMMA MATRIX, STATE FORCING MATRIX (5X3) 


c 


GAMT 


s 


TRANSPOSE OF GAMMA 


c 


GATE 


= 


H(K)*P(K/K-1)XH(K)T 


c 


GATES 


s 


3 SIGMA ADAPTIVE GATE, 3x( (GATE+R)XXl/2) 


c 


GAUSS 


s 


SUBROUTINE TO CALC GAUSSIAN PSEUDO-RANDOM 


c 






NUMBER GENERATOR 


c 


GE 


s 


GXE 


c 


GFLAG 


s 


USED WITH ADAPTIVE GATE, O-CONTINUE 


c 






1-GATE3 EXCEEDED, 2- REINITIALIZE PROBLEM 


c 


H 


s 


MEASUREMENT MATRIX (5X1) 


c 


HE 


= 


A PRIORI HEADING INFORMATION 


c 


HT 


= 


TRANSPOSE OF H 


c 


I 


= 


COUNTER 


c 


INIT 


= 


INITIALIZATION FLAG TO DETERMINE WHICH P(0/-1) 


c 






TO USE 


c 


J 


= 


COUNTER 


c 


K 


= 


ITERATION INTERVAL 


c 


KK 


= 


K-1, COUNTER 


c 


KOUNT 


= 


COUNTER TO DETERMINE THE TIMES THE ADAPT GATE 


c 






IS EXCEEDED 


c 


L 


= 


ROW OR COLUMN OF MATRIX, 1 IN THIS CASE 


c 


LD 


= 


MAX NUMBER OF ROWS OR COLUMNS 


c 


M 


= 


ROW OR COLUMNS OF MATRIX 


c 


MD 


s 


MAX NUMBER OF ROWS OR COLUMNS 
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c 


MFLAG 


= 


USE MANEUVERING EQUATIONS, Ql(K) 


c 


MZ 


= 


LAST TRACK VALUE OF DATA 


c 


MZZ 


= 


MZ-1 


c 


N 


= 


RONS OR COLUMNS OF MATRIX 


c 


NB 




COUNTER FOR BUOY NUMBER 


c 


NBUOY 


= 


TOTAL NUMBER OF BUOYS IN THE PATTERN 


c 


NO 




MAX NUMBER OF RONS AND COLUMNS 


c 


NFLAO 


= 


INDICATES 0.0- NO NOISE IS ADDED TO 


c 






MEAS, 1.0 NOISE IS INCLUDED 


c 


NM 


= 


USED NITH EXP VAR, 10 TERM MOVING NINDON 


c 






NM=K-10 


c 


NNZ 


s 


NZ + 10 


c 


NZ 


= 


FIRST TRACK INTERVAL VALUE, USUALLY 1 OR 60 


c 


NZZ 


= 


NZ-1 


c 


OFF 


= 


ERROR IN POSITION, IN YARDS 


c 


PFLAG 


= 


0.0-CALC P(0/-1) FROM A PRIORI INFORMATION 


c 






1.0-CALC P(K/K-1) FROM PREVIOUS SIMULATION 


c 


PHI 




TRANSITION MATRIX (5 X 5) 


c 


.PHIGAM 


= 


SUBROUTINE TO CALC PHI AND GAMMA 


c 


PHIT 


s 


TRANSPOSE OF PHI 


c 


PI 


s 


3.1415927 


c 


PI180 


= 


PI/180 DEGS 


c 


PKK 


= 


PCK/K) COVARIANCE OF ERROR MATRIX 


c 


PKKMl 


= 


P(K/K-1) PREDICTED COVARIANCE OF ERROR MATRIX 


c 


PLOTB 


= 


SUBROUTINE TO PLOT BEARING MEAS. GAINS AND 


c 






COV OF ERROR 


c 


PLOTF 


= 


SUBROUTINE TO PLOT FREQ MEAS GAINS AND COV 


c 






OF ERROR 


c 


PLOTGB 


= 


SUBROUTINE TO PLOT ADAPT GATE AND PREDICTED 


c 






RESIDUAL FROM BEARING MEAS 


c 


PLOTGF 


= 


SUBROUTINE TO PLOT ADAPT GATE AND PREDICTED 


c 






RESIDUAL FROM FREQ MEAS 


c 


PROD 


= 


SUBROUTINE TO MULTIPLY MATRICES 


c 


Q 


= 


STATE EXCITATION MATRIX (5 X 5) 


c 


QFIND 


= 


SUBROUTINE TO CALC Q 
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c 


R 




MEAS NOISE COVARIANCE OF ERROR MATRIX 


c 


RFLAG 


= 


CALCS THE NUMBER OF TIMES THE PROBLEM IS 


c 






REINITIALIZED FOR A GIVEN MEASUREMENT 


c 


RFREQ 


= 


TRUE RADIATED FREQUENCY 


c 


RSTART 


= 


SUBROUTINE TO REINITIALIZE THE PROBLEM 


c 


SIGB 


= 


STANDARD DEVIATION FOR BEARING MEAS NOISE, RADS 


c 


SIGDG 


= 


STD DEV FOR BEARING MEAS NOISE, DEGS 


c 


SI6F 


= 


STD DEV FOR FREQ NOISE, HZ 


c 


SIGFE 


3 


STD DEV FOR A PRIORI FREQ, HZ 


c 


SIGHE 


= 


STD DEV FOR A PRIORI HEADING, DEGS 


c 


SIGMA 


3 


VECTOR OF 3 STD DEVS FROM QFIND 


c 


SIGPOS 


= 


STD DEV FOR A PRIORI POSITION, NM 


c 


SIGVE 


3 


STD DEV FOR A PRIORI VELOCITY, KTS 


c 


SIGVXE 


= 


STD DEV FOR VX COMP OF VE,KTS 


c 


SIGVYE 


3 


STD DEV FOR VY COMP OF VE,KTS 


c 


SUMX 


= 


SUMMATION FOR 10 TERM MOVING HINDOW FOR 


c 






X COMP 


c 


SUMY 


= 


SUMMATION FOR 10 TERM MOVING WINDOW FOR 


c 






Y COMP 


c 


TEMP 


3 


MATRIX USED FOR CALCULATIONS 


c • 


TEMPI 


3 


MATRIX USED FOR CALCULATIONS 


c 


TEMP2 


= 


MATRIX USED FOR CALCULATIONS 


c 


TEMPS 


= 


MATRIX USED FOR CALCULATIONS 


c 


TGATE 


= 


COUNTS THE OF TIMES GATES IS EXCEEDED 


c 


THDG 


3 


TRUE HEADING OF TARGET 


c 


TIME 


= 


TIME OF MEAS 


c 


TWOPI 


3 


2XPI 


c 


UNIT 


= 


IDENTITY MATRIX 


c 


VARX 


= 


X COMP EXPERIMENTAL VARIANCE 


c 


VARY 


= 


Y COMP EXPERIMENTAL VARIANCE 


c 


VE 


= 


A PRIORI VELOCITY INFORMATION 


c 


VMEAS 


3 


MEAS VELOCITY IN DOPPLER EQUATION 


c 


VP 


3 


VELOCITY OF SOUND IN WATER, FT/SEC 


c 


VS 


3 


MATRIX OF THE VMEAS 
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c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 



VT 

vx 

VXE 

VY 

VYE 

H 

XB 

XO 

XDE 

XE 

XKK 

XKKMl 

XT 

YB 

YD 

YDE 

YE 

YT 

Z 

ZDBRG 

ZFREQ 

ZGATE 

ZZBRG 

ZZFREQ 



TARGET'S TRUE VELOCITY 
TARGET'S TRUE X COMP VELOCITY 
A PRIORI X COMP VELOCITY INFORMATION 
TARGET'S TRUE Y COMP VELOCITY 
A PRIORI Y COMP VELOCITY INFORMATION 
RANDOM FORCING FUNCTION COVARIANCE MATRIX 
BUOY'S X COMP POSITION, NM AND YARDS 
DISTANCE IN TARGET'S X COMP AND BUOY'S X COMP, 
XT-XB 

DISTANCE IN XE-XB 
A PRIORI X COMP INFORMATION 
ESTIMATED STATE VECTOR, X(K/K) 

PREDICTED STATE VECTOR, X(K/K-1) 

TARGET'S TRUE X COMP 

BUOY'S Y COMP POSITION, NM AND YARDS 

DISTANCE YT-YB 

DISTANCE YE-YB 

A PRIORI Y COMP INFORMATION 

TARGET'S TRUE Y.COMP 

MEAS MODEL 

BEARING MEAS IN DEGS 
MATRIX OF ZZFREQ 'S 

SUBROUTINE TO CALC GATES AND TEST PREDICTED 
RESIDUAL 

NOISY BEARING MEAS IN RADS 
NOISY FREQ MEAS IN HZ 



MXM VARIABLE DECLARATIONS MMM 



DIMENSION TIME(200),XT(200),YT(200),VT(200),THDG(200) 
DIMENSION XB(10),YB(10),BTYPE(10),BFLAG(10) 

DIMENSION VS(5,200),VX(200),VY(200) 

DIMENSION H(5,5),HT(5,5),G(5,5),PHI(5,5),TEMP(5,5),TEMP1(5,5) 
DIMENSION Q(5,5), PKK(5, 5) , PKKMl ( 5 , 5) , TEMP2(5, 5) , TEMP3(5, 5) 
DIMENSION GAM(5,5),UNIT(5,5),SIGMA(3) 
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DIMENSION Z(5),E(5),GE(5)>W(5,5),EXT(200),EYT(200) 
DIMENSION GAMT(5,5),PHIT(5,5),XKIC(5),XKKM1(5) 

DIMENSION ZDBRG(5,200),DBRG(5,200),FD(5,200),ZFREQ(5,200) 
REAU(8 DSEED 
CHARACTERX5 BTYPE 
C 

C INITIALIZE TERMS 

C K=DISCRETE PT IN TIME, THE STAGE OF THE PROCESS 
C L-1 ROH OR COLUMN 

C M-2 ROH OR COLUMNS 

C N-5 RONS OR COLUMNS 

C LD=MD=ND-NAX t OF ROMS OR COLUMNS 
C DT*DELAY TIME 

C MZ=NO. OF TRACK VALUES 

C MZZ*MZ-1 USE IN DO LOOP 

NZ=1 
MZ=60 
PFLAG*0.0 
NZZ=NZ-1 
MZZ*MZ-1 
NNZ=NZ+10 
L=1 
M=3 
N=5 
LD=5 
MD=5 
ND=5 

PI180=0.017A533 

PI=3.1A15927 

TH0PI=2»PI 

RFREQ=300. 

VP=A860. 

FD0P=0. 

DSEED=50519 
C FLAGS 
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C NOISE FLAG-0.0 NOISE OFF, 1.0 NOISE ON 
NFLAG=1.0 

C MANV FLAG-0.0 MANV OFF, 1.0 MANV ON 
MFLAG=1 .0 

C ADAPT GATING FLAG-0.0 OFF, 1.0 ON 
AFLAG=1.0 

C INITIALIZE FLAGS 

GFLAG^O. 

FLAG*0. 

C NUMBER OF INITIAL PKKMl MATRIX TO USE 

INIT=4 

C INITIAL ESTIMATE OF TARGET POSITION, VELOCITY, AND COURSE IN 

C NM, KTS, AND DEGS. 

IF(PFLAG.EQ.O.O) THEN 
XE=10. 

YE=12.0 

VE=5.0 

HE=180. 

C STD. DEV. FOR PKKM(0/-1) 

C POSIT=0.5NM,VEL=3KTS,HEADING=10DEG,FREQ=1 HZ 

SIGPOS=0.5 
SIGVE=3.0 
SIGHE=10 
SIGFE=1.0 

C CONVERT XE AND YE TO YARDS 

XE=XE»2025.3667 
YE=YE»2025.3667 
SIGP0S=SIGP0SX2025 . 3667 

C KTS TO YARDS/MIN 

VE=VEX33. 75633 
SIGVE=SIGVEX33. 75633 

C CALC VXE AND VYE OF THE TARGET 

VXE=VEXSIN(HEXPI180) 

VYE=VEXC0S(HEXPI180) 

SIGVXE=SIGVEXSIN((HE+SIGHE)xpI180) 



157 



SIGVYE=SIGVEXC0S((HE+SIGHE)»PI180) 

ENDIF 

C 3 SIGMA GATE AND RESTART COUNTER FOR ADAPT. GATING 
GATE3=0. 

K0UNT=0 

C 

C 

C CONVERT VP (VEL OF SOUND IN WATER) FROM FT/SEC TO YARDS/MIN 
VP=VP3t60. 0/3.0 
C 
C 

C READ IN ACTUAL TARGET DATA 

C 

READ(3,1)(TIME(I),XT(I),YT(I),VT(I),THD6(I),I=NZ,MZ) 

1 F0RMAT(F8.2,4X,Fll.<i,<iX,Fll.‘i,‘iX,Fll.<i,4X,F7.2) 

C WRITE ACTUAL TRACK VALUES 

C WRITE(8,1000) 

1000 F0RMAT(<iX,*TIMEM2X,*XTM2X, *YTM0X,*T6T VEL*,6X,*TGT HDG*) 
C DO LOOP CONVERTS VT(I) FROM KTS TO OTHER UNITS 
DO 800 I=NZ,MZ 

C CONVERT VT TO AGREE WITH VP 

C KTS TO YARDS/MIN 

VT(I)=VT(I)*33. 75633 
C CONVERT XT AND YT TO YARDS 

XT(I)=XT(I)*2025.3667 
YT(I)=YT(I)*2025.3667 
800 CONTINUE 

C 

WRITE(8,1)(TIME(I),XT(I),YT(I),VT(I),THD6(I),I=NZ,MZ) 

ClOOl FORMATC F8 . 2, 3(<iX, Fll . <i) , 4X, F7 . 2) 

C 

C READ IN NUMBER OF BUOYS, THEN READ TYPE AND LOCATION 
READ(2,5) NBUOY 
5 F0RMATCI3) 

C BFLAG=1.0 BUOY GIVES BEARING AND FREQ 
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C BFLAG=2.0 BUOY GIVES FREQ ONLY 
C READ(4,6)(BTYPE(I),XB(I),YB(I),I=1,NBU0Y) 

READ(2,6)(BTYPE(I),BFLAG(I),XB(I),YB(I),I*l,NBUOY) 

6 FORMATC 2X, A5, 2X, F2 . 0 , 2X, F8 . A , AX, F8 . A) 

C WRITE ACTUAL BUOYS -TYPES AND LOCATIONS 
C CONVERT XB AND YB TO YARDS 
DO lOOA I-l,NBUOY 
XB(I)*XB(I)»2025.3667 
YB(I)*YB(I)»2025.3667 
lOOA CONTINUE 

WRITE(8,1005) 

1005 FORMAT(//,10X, ’BUOYS*) 

WRITE(8,1006) 

1006 F0RMATC3X, »TYPE’,3X, »FLAG»,8X, »XB*,13X, »YB») 

WRITE(8,7) (BTYPE(I),BFLAG(I),XB(I),YB(I),I*l,NBUOY) 

7 F0RMAT(3X,A5,3X,F2.0,3X,F11.A,AX,F11.A) 

C 

C CALC. THE ACTUAL BEARINGS FROM BUOYS TO TARGET TRACKS AND THE 

C FREQS. RECEIVED 

C 

C WRITE BEARINGS AND FREQS FOR EACH BUOY 

DO 102 I=NZ,MZ 
C WRITE(10,1008) 

1008 FORMATC//,* POSIT » , AX, ’XT » , IIX, »YT * ) 

C WRITE(10,1009) I,XT(I),YT(I) 

1009 F0RMAT(IA,2X,2(F11.A,2X)) 

C WRITE(10,1010) 

1010 F0RMAT(/,7X, ’XB’,11X, ’YB’,7X, ’BRG’,8X, ’FREQ’) 

DO 103 J=l,NBUOY 

XD=XT(I)-XB(J) 

YD=YT(I)-YB(J) 

CALL BEAR(XD,YD,BRG) 

DBRG(J,I)=BRG/PI180 
C CALC VX AND VY OF THE TARGET 

VX(I)=VT(I)*SIN(THDG(I)*PI180) 
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VY(I)=VT(I)*COS(THD6(I)*PI180) 

CALL DOPP(VX,VY,XD,YD,VP,RFREQ,VMEAS,FDOP,I) 

FD(J,I)=FDOP 

VS(I,J)=VMEAS 

C WRITE(10,1010) XB(J),YB(J),XT(I),YT(I),DBRG(J,I),FD(J,I) 

C WRITE(10,1011) XB(J),YB(J),DBRG(J,I),FD(J,I) 

1011 F0RMAT(2(F11.A,2X),F7.2,2X.F10.A) 

103 CONTINUE 
102 CONTINUE 

C STARTS KALMAN FILTER PART OF PROGRAM 
C 

K=NZ 

DO 899 1=1, M 
DO 899 J=1,M 

899 H(I,J)=0.0 
DO 900 1=1, N 
DO 900 J=1,N 

PHI(I,J)=0. 

PHI(I,I)=1.0 

PKKM1(I,J)=0.0 

900 CONTINUE 

DO 20 1=1, N 
DO 25 J=1,M 
GAM(I,J)=0. 

25 CONTINUE 

20 CONTINUE 

IF(PFLAG.EQ.O.O) THEN 

XKKM1(1)=XE 

XKKM1(2)=VXE 

XKKM1(3)=YE 

XKKMK<f)=VYE 

XDE=XE-XB(1) 

YDE=YE-YB(1) 

CALL D0PP(VXE,VYE,XDE,YDE,VP,RFREQ,VMEAS,FD0P,1) 
XKKM1(5)=FD0P 
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c 

C << INITIAL PKKMl MATRICES ARE SET UP USE 1 
C IF CONTINUING FROM A PREVIOUS RUN PFLAG =1.0 

C AND THE PKKMl MATRIX HILL BE READ IN 

IF(INIT.EQ.l) THEN 
C INITIAL #1 

PKKM1(1,1)=1000. 

PKKM1(2,2)=500. 

PKKM1(3,3)=1000. 

PKKM1(A,A)=500. 

C 

ELSEIF(INIT.EQ.2) THEN 
C INITIAL #2 

C POSITION (1NM)XX2, VEL. (2KTS)«2 

PKKMl (1,1)=A.0E6 
PKKM1(2,2)=4.0E3 
PKKM1(1,2)=1.AE4 
PKKM1(2,1)*PKKM1(1,2) 

PKKM1(3,3)=A.DE6 

PKKM1(3,A)=1.4E4 

PKKM1(4,3)=PKKM1(3,4) 

PKKM1(4,4)=4.0E3 

ELSE 

C INITIAL f3 

C POSITION (.5NM)»X2, VEL. (3KTS)**2 

PKKMl (1,1)=SIGP0S 
PKKM1(2,2)=SIGVXE 
PKKM1(3,3)=SIGP0S 
PKKM1(4,4)=SIGVYE 
ENDIF 
C 

PKKM1(5,5)=SIGFE 

ELSE 

READ(2,91) TIME(NZZ) 

DO 7235 1=1, N 
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7235 READ(2,X) (PKKCI, J),J=1»N) 
HRITE(8,656) 

DO 7323 1=1, N 

7323 WRITE(8,92) (PKKCI, J), J=1,N) 

READ(2,M) (XKK(I),I=1,N) 
HRITE(8,8011) 
WRITE(8,92)(XKK(J),J=1,N) 

GO TO 67 
ENDIF 
C 

C HRITE(8,7177) K 

7177 FORMAT(//,»tttttttttttt K=',I<i) 
HRITE(8,555) 

555 FORMAT(/» PKKMl MATRIX *) 

DO 3022 1=1, N 

3022 HRITE(8,91) (PKKMKI, J),J = 1,N) 

HRITE(8,8812) 

8812 FORMAT (/* XKKMl ») 

WRITE(8,91) (XKKM1(J),J=1,N) 

C 

C 

91 F0RMAT(8(1PE12.<»)) 

92 F0RMAT(2X,8(1PE12.5,2X)) 

67 CONTINUE 

C NB IS COUNTER INDICATING BUOY NUMBER 

NB=1 
C 

IF(K.EQ.l) GO TO 3 
HRITE(8,7177) K 
DT=TIME(K)-TIME(K-1) 

HRITE(8,7905) DT 

7905 F0RMAT(5X,» DT= »,F10.2) 

CALL PHIGAM(DT,N,M,PHI,GAM,K) 

C HRITE PHI MATRIX 

HRITE(8,979) 
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979 FORMATC/* PHI MATRIX *) 

DO 3580 1=1, N 

3580 WRITE(8,92) (PHKI, J), J = 1,N) 

C HRITE GAMMA MATRIX 

WRITE(8,978) 

978 F0RMAT(/» GAMMA MATRIX ») 

DO 3581 1=1, N 

3581 HRITE(8,92) (GAM(I, J), J=1,M) 

CALL PR0D(PHI,XKK,N,N,L,XKKM1,N,M,L) 

HRITE(8,8812) 

HRITE(8,92) (XKKMl(J), J=1,N) 

CALL QFIND(DT, GAM, XKKM1,W,Q,N,M,ND,MD, SIGMA, K,MFLAG,GFLAG) 

C WRITE(8,5AA) 

544 F0RMAT(/» W •) 

C DO 3021 1=1,3 

C3021 HRITE(8,92) (W(I, J), J=l,3) 

WRITE(8,799) 

799 F0RMAT(/» Q MATRIX •) 

DO 3123 1=1, N 

3123 WRITE(8,92) (Q(I, J), J=1,N) 

C 

3 IF((BFLAG(NB).EQ.1.0).0R.(BFLAG(NB).EQ.2.0)) THEN 

RFLAG=0.0 
TGATE=0.0 
NRITE(8,3583) NB 

3583 F0RMAT(//,5X, *FREQ MEAS FROM BUOY »,I2) 

CALL FMEAS(XB,YB,XKKM1,VP,H,R,N,NB,K,FDKKM1,D) 

8 CALL GAIN( PKK,PKKM1,Q,R, PHI, H,N,L,G,ND,MD,LD,K, FLAG, GATE, GFLAG,NZ) 

NRITE(8,2400) GATE 
2400 FORMAT(/»GATE =*,1PE12.5) 

C 

HRITE(8,656) 

656 FORMAT(/» PKK •) 

DO 3023 1=1, N 

3023 WRITE(8,92) (PKKd, J), J = 1,N) 
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c 

C SOLVE XKK=XKKM1+G(K)(Z(1)-FDKKM1) 

C 

IFCGFLAG.EQ.l. ) GO TO 5110 
C CALC FREQ. CONFIDENCE LEVELS (IE STD DEV) 

D*D/2025.3667 
IFCD.LE.2. )THEN 
C SIGF=0.02 

SIGF-0.04 

ELSEIF(D.LE.5.0) THEN 
C SIGF=O.OA 

SIGF=0.06 

ELSEIF(D.LE.IO.O) THEN 

SIGFsO.OS 

ELSE 

SIGF*0.1 

ENDIF 

WRITE(8,5009) SIGF 

5009 FORMATC/, »FREQ. STD DEV **,FA.2) • 

C ADD NOISE TO FREQ MEAS. 

CAL L GAUSSC DSEED, SIGF, FD( NB, K) , ZZFREQ, NFLAG) 
ZFREQ(NB,K)=ZZFREQ 
C 

5110 E(1)=ZFREQ(NB,K)-FDKKM1 
WRITE(8,8811) 

C WRITE(10,8811) 

8811 F0RMAT(/,8X, 'Z'^lOX, *ZKKM1*,8X, 'ACTUAL*) 
WRITE(8,92) ZFREQ(NB,K),FDKKM1,FD(NB,K) 

C WRITE(10,92) ZFREQ(NB,K),FDKKM1,FD(NB,K) 

WRITE(8,3029) 

C PRINT OUT ERROR 

WRITE(8,92) E(l) 

GFLAG=0. 

C 

C AFLAG=1.0 USE ADAPTIVE FILTER 
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C AFLAG=0. DON'T USE ADAPTIVE FILTER 

IF(AFLAG.EQ.l. ) THEN 

C DETERMINES IF RESIDUAL IS INSIDE GATES 

C GATE1=(HXPKKM1XH*+R)KK0.5 

C GATE3=3KGATE1 

IF(K.NE.l) THEN 

CALL ZGATEC E( 1) , GATE, R, W, GFL AG, KOUNT, GATES) 
IF(GFLAG.EQ.l.O) TGATE=TGATE+1 . 0 
IF(KOUNT.EQ.S) THEN 
CALL RSTART(XB,XKKM1 , PKKMl , RFLAG) 

GFLAG=2. 

RFLAG=RFLAG+1.0 

WRITE(8,987) 

987 FORMATC/, •KKKKKKKXK RESTART THE PROBLEM KXKMXXXXXX*) 
K0UNT=0 

C WRITE XKKMl 

WRITE(8,8812) 

WRITE(8,92) (XKKM1(J),J-1,N) 

C WRITE PKKMl 

WRITE(8,555) 

DO 3024 1=1, N 

3024 WRITE(8,92) (PKKMKI, J), J = 1,N) 

ENDIF 

IF(GFLAG.NE.O.) THEN 

CALL QFINDCDT, GAM, XKKMl, W,Q,N,M,ND,MD, SIGMA, K,MFLAG,GFLAG) 
C WRITE(8,544) 

C 544 FORMAT(/* W •) 

C DO 3025 1=1,3 

C3025 WRITE(8,92) <W( I , J ) , J=1 , 3) 

WRITE(8,799) 

DO 3124 1=1, N 

3124 WRITE(8,92) <Q(I, J), J=1,N) 

GFLAG=1. 

GO TO 8 
ENDIF 
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ENDIF 

IF(KOUNT.GT.O) KOUNT=KOUNT-l 
ENDIF 

CALL PROD(G.E,N,L.L,GE,ND,MD) 

CALL ADD(XKKN1,GE.N.L.XKK,ND,MD) 

HRITE(8.8011) 

8011 FORMATC/* XKK *) 

HRITE(8,92)(XKK(J). J=1,N) 

C MRITE(8^9897) K,NB 

9897 FORMATC//* MMMM* SUMMARY FOR K- *,IA,» FROM BUOY » , 12, *X»»*M* ) 
C WRITE(8,9899) 

EXT(K)sXKKCl) 

EYT(K)=XKK(3) 

VARX-0.0 

VARYsO.O 

SUMXsO.O 

SUMYsQ.O 

IFCK.GE.MNZ) THEN 
NM*K-10 

DO 3033 I=NM,K 

SUMX=SUMX+(XT(I)-EXT(I))*3<2 
SUMY=SUMY+(YT(I )-EYT( I ))**2 
3033 CONTINUE 
ENDIF 

VARX=SUMX/9. 

VARY=SUMY/9. 

C 

C SET UP ARRAYS TO COLLECT GAINS, VARIANCES, AND ERROR ELLIPSOID 
C DATA FOR PLOTS. 

C 

CALL PLOTFCTIME, EXT, EYT,G,PKK,K,NB,NBUOY,NZ,MZ,VARX, VARY) 

C STORE RESIDUAL AND GATES VS TIME FOR PLOTS 

CALL PLOTGF(TIME,K,NB,NBUOY,NZ,MZ,ABS( EC D), GATES, TGATE,RFLAG) 

C 

9899 F0RMATC5X, *TIME*,9X, *XT*,11X, *YT*,12X, *EST XT*,11X, *EST YT*) 
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C WRITE(8,393) ( TIME( I ) , XT( I ) , YT( I) , EXT( I) , EYT( I) , 1=1 , K) 

WRITE(8,393) TIME(K),XT(K),YT(K),EXT(K),EYT(K) 

WRITE(10,393) TIME(K) ,XT(K) , YT(K) , EXT(K) , EYT(K) 

393 F0RMAT(/,F8.2,<»X,F11.3,^X,F11.3,4X,F11.3,^X,F11.3) 

ENDIF 

C 

C BEARING MEASUREMENT 

C 

IF((BFLAG(NB).EQ.1.0).0R.(BFLAG(NB) .EQ.3.0)) THEM 
C WRITE(8,358A) NB 

C3584 FCRMAT(//,5X, 'BEARING MEAS. FROM BUOY ',12) 

IF(BFLAGCNB) .EQ.1.0) THEN 
FLAG=1.0 

C FLAG=1.0 MEANS REMAIN IN SAME TIME SLOT IE. K REMAINS THE SAME 
C 

DO AOOO 1=1, N 
AOOO XKKM1(I)=XKK(I) 

MRITE(8,8812) 

WRITE<8,92) <XKKM1(J),J=1,N) 

DO AOOl 1=1, N 
DO AOOl J=1,N 

4001 PKKMKI, J)=PKK(I,J) 

WRITE(8,555) 

DO 4002 1=1, N 

4002 WRITE(8,92) (PKKMKI, J), J = 1,N) 

ELSE 

FLAG=0.0 

ENDIF 

WRITE(8,3584) NB 

3584 F0RMAT(//,5X, 'BEARING MEAS. FROM . BUOY ',12) 

CALL BMEAS(XB,YB,XKKM1,VP,H,R,N,NB,K,BRKKM1,D) 

RFLAG=0.0 

TGATE=0.0 

4 CALL GAIN( PKK, PKKM1,Q,R, PHI, H,N,L,G,ND,MD,LD,K, FLAG, GATE, GFLAG,NZ) 
NRITE(8,2400) GATE 
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c 



c 

WRITE(8,656) 

DO 4003 1=1, N 

4003 WRITE(8,92) (PKK(I,J), J=1,N) 

C 

C SOLVE XKK=XKKM1+G(K)(Z(1)-BRKKM1) 

C 

IF(GFLAG.EQ.l.) GO TO 5111 
C CALC BEARING CONFIDENCE LEVELS 

D=D/2025.3667 
1F(D.LE.2.)THEN 
C S1GDB=2.0 

S1GDB=5.0 

ELSE1F(D.LE.5.0) THEN 
C S1GDB=5.0 
S1GDB=10.0 

ELSEIF(D.LE.IO.O) THEN 

S1GDB=10.0 

ELSE 

S1GDB=15.0 

ENDIF 

S1GB=S1GDBMP1180 
MR1TE(8,5010) SIGDB 

5010 FORMAT(/, 'BEARING STD DEV = ',F4.2) 

C ADD NOISE TO BRG MEAS. 
BRG=DBRG(NB,K)XP1180 

CALL GAUSSC DSEED, SIGB, BRG, ZZBRG, NFLAG) 
Z(1)=ZZBRG 

1F( BRKKMl . LT . 0 . ) BRKKM1=BRKKM1+TW0P1 
E(1)=Z(1)-BRKKM1 
1F(E(1).GT.P1) E(1)=E(1)-TW0P1 
1F(E(1).LT.-P1) E(1)=E(1)+TW0P1 
5111 WR1TE(8,8811) 

C WR1TE(10,8811) 
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NR1TE(8,92) Z( 1 ) , BRKKMl , BRG 
DBKKM1=BRKKM1/PI180 
ZDBRG(NB,K)=Z(1)/PI180 

WR1TE(8,93) ZDBRG(NB,K).DBKKM1,DBRG(NB,K) 

C WR1TE(10.94) ZDBRG(NB,K),DBKKM1,DBRG(NB,K) 

93 FORMAK/, 5X, F7 .2, 5X, F7 . 2, 5X, F7 . 2) 

94 FORMATC/, 5X, F7 .2, 5X, F7 . 2, 5X, F7 . 2) 

WRITE(8,3029) 

3029 FORMATC/ ' ERROR xxxxxxxxxxxxxxxxxxxxxxxxxxx •) 
WRITE(8,92) .Ed) 

GFLAG=0. 

C AFLAG=1.0 USE ADAPTIVE FILTER 

C AFLAG=0. DON'T USE ADAPTIVE FILTER 

IFCAFLAG.EQ.l.) THEN 

C DETERMINES IF RESIDUAL IS INSIDE GATES 

C GATE1S(HXPKKM1XH»+R)XX0.5 

C GATE3=3»GATE1 

IF(K.NE.l) THEN 

CAL L ZGATEC E( 1 ) , GATE, R, W, GFLAG, KOUNT, GATES) 
IF(GFLAG.EQ.l.O) TGATE*TGATE+1 . 0 
IF(KOUNT.EQ.S) THEN 
CALL RSTART(XB,XKKM1,PKKM1) 

GFLAG=2.0 

RFLAG=RFLAG+1. 

WRITE(8,987) 

C 987 FORMATC/, 'xxxxxxxxx RESTART THE PROBLEM xxxxxxxxxx*) 

K0UNT=0 

C WRITE XKKMl 

WRITE(8,8812) 

WRITE(8,92) (XKKM1(J),J=1,N) 

C WRITE PKKMl 

WRITE(8,555) 

DO 3026 1=1, N 

3026 WRITE(8,92) (PKKMKI, J), J = 1,N) 

ENDIF 



IFCGFLAG.NE.O. ) THEN 

CALL QF1N0(0T,GAM»XKKM1,N,Q,N,M,ND, MO, SIGMA, K,MFLAG,GFLAG) 
C WRITE(8,544) 

C 544 FORMATC/* H *) 

C DO 3027 1*1,3 

C3027 WRITE(8,92) (W(I, J) , J=l,3) 

WRITE(8,799) 

DO 3125 1*1, N 

3125 MRITE(8,92) (Q(I, J), J*1,N) 

GFLAG*1. 

GO TO 4 

ENDIF 

ENDIF 

IF(KOUNT.GT.O) KOUNT*KOUNT-l 
ENDIF 

CALL PROD(G,E,N,L,L,GE,ND,MD) 

CALL ADD(XKKM1,GE,N,L,XKK,ND,MD) 

C PRINT OUT XKK 

MRITE(8,8011) 

NRITE(8,92)(XKK(J),J*1,N) 

C NRITE(8,9897) K,NB 

C WRITE(8,9899) 

EXT(K)=XKK(1) 

EYT(K)*XKK(3) 

C 

VARX*0.0 

VARY*0.0 

SUMX*0.0 

SUMY=0.0 

IF(K.GE.NNZ) THEN 
NM=X-10 

DO 3034 I*NM,K 

SUMX=SUMX+( XT( I)-EXT( I ) )»»2 
SUMY=SUMY+(YT(I)-EYT(I))»»2 
3034 CONTINUE 
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ENDIF 

VARX=SUMX/9. 

VARY=SUMY/9. 

C SET UP ARRAYS TO COLLECT GAINS, VARIANCES, AND ERROR ELLIPSOID 
C DATA FOR PLOTS. 

C COMMENT PLOTB OUT IF HANT ONLY VEL. ERROR ELLIPSES ONLY FROM 
C PLOTF. 

C 

CALL PLOTB(TIME, EXT, EYT,G,PKK,IC,NB,NBUOY,Nr,MZ,BFLAG,VARX, VARY) 
C 

C STORE RESIDUAL AND GATES VS TIME FOR PLOTS 

CAL L PLOTGBCTIME, K, NB, NBUOY, NZ, MZ, ABS( E( 1 ) ), GATES, TGATE, RFLAG) 

C 

C WRITE(«,S9S) (TIME(I),XT(I),YT(I),EXT(I),EYT(I),I=NZ,IO 
WRITE(«,S9S) TIME(K),XT(IO,YT(IO,EXT(K),EYT(K) 

WRITE(10,S9S) TIME(K),XT(K),YT(IO,EXT(K),EYT(K) 

ENDIF 

IFCNB.LT. NBUOY) THEN 
NB=NB+1 
FLAG*1.0 
DO 5000 1=1, N 

5000 XKKM1(I)=XKK(I) 

MRITE(8,8812) 

WRITE(8,92) (XKKM1(J),J=1,N) 

DO 5001 1=1, N 
DO 5001 J=1,N 

5001 PKKM1(I,J)=PKK(I,J) 

WRITE(8,555) 

DO 5002 1=1, N 

5002 WRITE(8,92) (PKKM1(I,J), J=1,N) 

C 

C LOOP BACK TO CALC FREQ AND BEARING FROM NEXT BUOY 
GO TO S 
ENDIF 

C COMMENT OUT PLOTF AND PLOTB ABOVE. THE CALL BELOW WILL WRITE 
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C THE LAST ELLIPSE CALC. BY PLOTF OR PLOTB. 

IF(BFLAG(NB) .EQ.2. ) THEN 

CALL PLOTF(TIME, EXT, EYT,G,PKK,K,NB,NBUOY,NZ,MZ,VARX, VARY) 

ELSE 

CALL PLOTB(TIME, EXT, EYT,G,PKK,K,NB,NBUOY,NZ,MZ,BFLAG,VARX, VARY) 

ENDIF 

K*K+1 

KK=K-1 

IF(K.GT.MZ) GO TO 888 
GO TO 67 
C 

C SET UP FILES TO PLOT 
888 MRITE(8,9897) KK,NB 

DO 104 I-NZ,HZ 
WRITE(10,1008) 

WRITE(10,1009) I,XT(I),YT(I) 

WRITE(10,1013) 

1013 FORMAT!/, 6X, *XB» , 9X, »YB • , 9X, *BRG»,6X, 'ZDBRGSTX, *FREQ*,7X, 
**ZFREQ*) 

DO 105 J-l,NBUOY 

WRITE! 10, 1014) XB!J),YB!J),DBRG!J,I),ZDBRG!J,I),FD!J,I),ZFREQ!J,I) 
1014 F0RMAT!2!F11.4,2X),F7.2,2X,F7.2,2X,F10.4,2X,F10.4) 

105 CONTINUE 
104 CONTINUE 

WRITE!8,9899) 

DO 6000 I=NZ,KK 

0FF=!!XT!I)-EXT!I))»»2+!YT!I)-EYT!I))xx2)»3t0.5 
WRITE!8,393) TIME!I),XT!I),YT!I),EXT!I),EYT!I) 

C WRITE!10,92) XT! I ) , YT! I ) , EXT! I ) , EYT! I ) , OFF 
WRITE!4,92) XT! I ) , YT! I ) , EXT! I ) , EYT! I ) , OFF 
6000 CONTINUE 

C PRINT OUT GAINS, VARIANCES DATA FOR PLOTS 

CALL PLOTF!TIME, EXT, EYT, G,PKK,K,NB,NBUOY,NZ,MZ,VARX, VARY) 

C PLOT THE RESIDUAL AND GATE3 VS TIME 

CALL PL0TGF!TIME,K,NB,NBU0Y,NZ,MZ,E!1),GATE3,TGATE,RFLAG) 
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CALL PLOTB( TIME, EXT, EYT,G,PKK,K,NB,NBUOY,NZ,MZ,BFLAG,VARX, VARY) 
C PLOT THE RESIDUAL AND GATES VS TIME 

CALL PLOTGB(TIME,K,NB,NBUOY,NZ,MZ, Ed), GATES, TGATE,RFLAG) 
WRITE(4,5) NBUOY 

WRITE(A,6001) (BFLAG(I),XB(I),YB(I),I*l,NBUOY) 

6001 F0RMAT(SX,F2.0,SX,Fll.<i,4X,F11.4) 

WRITE(4,5) INIT 
C WRITE(5,SAS) 

WRITE(2,91) TIME(KK) 

S4S FORMAT!/* PKK *) 

DO SS2S 1=1, N 

SS2S WRITE(2,91) (PKKCI, J), J=1,N) 

WRITE(2,91)(XKK(J),J=1,N) 

C 

STOP 

END 

C 

C 

C 

SUBROUTINE BEAR (XD,YD,BRG) 

C XXX PURPOSE XXX 

C THIS GIVES ACTUAL BEARINGS FROM A BUOY TO THE TARGET IN RADIANS. 

C USING NORTH AS S60 DEGS., EAST AS 90 DEGS, SOUTH AS 180 DEGS., 

C NEST AS 270 DEGS. 

C 

C XXX VARIABLE DEFINITIONS XXX 

C SAME AS MAIN PROGRAM 

C 

PI180=0.01745SS 
PI=S.1A15927 
TNOPI=6. 283185 
IF(YD.EQ.O.O) THEN 
IF(XD.GT.O.O) THEN 
BRG=90.0XPI180 
ELSE 
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BRG=270.0XPI180 
ENDIF 
GO TO 1 
ENDIF 

BRG=ATAN2(XD,YD) 

IF(BRG.LT.O.O) BRG=TWOPI+BRG 
I RETURN 

END 
C 

SUBROUTINE DOPP (VX,VY,XD,YD, VP,RFREQ,VMEAS,FD, I) 
C XXX PURPOSE XXX 

C SUBROUTINE CALCS. THE DOPPLER FREQ. 

C 

C XXX VARIABLE DEFINITIONS xxx 

C SAME AS MAIN PROGRAM, EXCEPT 

C R » RANGE(DISTANCE) 

C 

C XXX VARIABLE DECLARATIONS xxx 

DIMENSION VX(200),VY(200) 

R=(XDXXD+YDXYD)XX0 . 5 
VMEAS=( XDXVXC I )+YDxVY( I) )/R 
FD=RFREQ/(1+(VMEAS/VP)) 

RETURN 

END 

C 

C 

C 

C 

C 

SUBROUTINE PHIGAM(T,N,M,PHI,GAM,K) 

C XXX PURPOSE xxx 

C CALCULATE THE PHI AND GAMMA MATRICES 

C 

C 

C xxx VARIABLE DEFINITIONS xxx 



174 



C SAME AS MAIN PROGRAM 

C 

C XXX VARIABLE DECLARATIONSxxx 

DIMENSION PHI(5,5),GAM(5,5) 

C 

C SET UP PHI MATRIX 

92 F0RMAT(2X,8(1PE12.5,2X)) 

PHI(1,2)=T 

PHI(3,4)=T 

GAM(1,1)=(TxT)/2 

GAM(3.2)=GAM(ia) 

GAM(2,1)=T 

GAM(A,2)=T 

GAM(5,3)=T 

C REMOVE C»S TO GET PRINTOUT IF DESIRED 

C HRITE(3,35) 

C35 F0RMAT(/,5X, • PHI MATRIX *) 

C DO 100 I=1>N 

ClOO HRITE(8,92) (PHKI, J), J=1,N) 

C NRITE(8,<iO) 

40 F0RMAT(/,5X, • GAMMA MATRIX *) 

C DO 101 1=1, N 

ClOl HRITE(8,92) (GAMCI, J), J=1,M) 

RETURN 

END 

C 

C 

C 

C 

C 

SUBROUTINE GAINCPKK, PKKMl , Q, R, PHI , H, N, L, G, ND, MD, LD, K, FLAG, GATE, 
XGFLAG,NZ) 

XXX PURPOSE XXX 

C THIS SUBROUTINE COMPUTES THE OPTIMUM GAIN MATRIX AND THE 
C COVARIANCE 
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c 

c XX* VARIABLE DEFINITIONS xxx 

C SAME AS MAIN PROGRAM 

C 

C XXX VARIABLE DECLARATIONS xxxx 

DIMENSION H(5,5),HT(5,5),G(5,5),PHI(5,5),TEMP(5,5),TEMP1(5,5) 
DIMENSION Q(5,5), PKK(5,5),PKKM1(5,5),TEMP2(5,5),TEMP3(5,5) 
DIMENSION GAM(5,5)»UNIT(5,5) 

DIMENSION Z(5),E(5)«GE(5) 

DIMENSION GAMT(5,5),PHIT(5,5),XICIC(5)»XKKM1(5) 

C 

C 

IF(K.EQ.NZ) THEN 
DO 900 I-l.N 
DO 900 J-1»N 
UNIT(I,J)=0.0 
900 UNIT(I^I)-1.0 

ENDIF 

C REMAIN IN SAME TIME SO PHI AND GAM MATRICES ARE THE SAME 

IF((K.EQ.1).0R.(FLAG.EQ.1.0).0R.(GFLA6.EQ.1.0)) GO TO 8889 
C NOTE HERE PKKM1(I,J) = PCK/IC-l) 

C WHERE P<K/K-1)=PHIXP(K-1/K“1)XPHIT + Q 

C 

C CALC PKKMl 
C 

CALL TRANS(PHI,N,N,PHIT,ND,MD) 

CALL PROD(PKK,PHIT»N,N,N,TEMP,ND,MD,LD) 

CALL PRODCPHI, TEMP, N,N,N, TEMPI, ND,MD,LD) 

CALL ADD(TEMP1,Q,N,N, PKKMl, ND,MD) 

C 

8889 CONTINUE 

IF(GFLAG.EQ.l.O) THEN 

CALL ADD(PKKM1,Q,N,N, PKKMl, ND,MD) 

ENDIF 

WRITE(8,8777) FLAG,GFLAG 
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WRITE(8,555) 

FORMATC/* MATRIX PKKMl *) 

DO 3022 1=1, N 
3022 WRITE(8,92)(PKKM1(I,J),J=1,N) 

8777 FORMATC/' FLAG* ' , F<i . 2, 2X, 'GFLAG =’,F4.2) 

C 

C CALC GAIN GCK) 

C 

C GOO = P(IC/IC-1)MHTM(HMP(IC/|C-1)XHT + R)XX-1 

CALL TRANS(H,L,N,HT,ND,MD) 

WRITE(8,39) 

39 FORMATC H *) 

DO 22 1*1, L 

22 HRITE(8,92)(H(I,J),J*1,N) 

92 F0RMAT(2X,8(1PE12.5,2X)) 

C WRITE(8,36) 

36 FORMATC NT * ) 

C DO 23 1*1, N 

C23 WRITE(8,92)(HT(I,J),J*1,L) 

CALL PR0D(PKKM1,HT,N,N,1,TEMP,ND,MD,LD) 
MRITE(8,20) 

20 FORMATC PKKM1*HT •) 

DO 21 1=1, N 

21 WRITE(8,92) (TEMPCI, J), J=l, L) 

CALL PRODCH, TEMP, L,N,L, TEMPI, ND,MD,LD) 
NRirE(8,38) 

38 FORMATC H P NT •) 

DO 50 1=1, L 
DO 50 J=1,L 
GATE=TEMP1(I,J) 

TEMP2CI, J)=TEMP1(I,J) 

50 TEMP3(I,J)=TEMP1(I,J) 

DO 26 1=1, L 

26 WRITE(8,92)(TEMP3(I,J),J=1,L) 

TEMP3(1,1)=TEMP3(1,1)+R 
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WRITE(S,33S) 

338 FORMATC H P HT + R’) 

DO 22A I-1,L 

224 WRITE(8,92)(TEMP3(I,J),J*1,L) 

C WRITE(8,31) 

C31 FORMATC* (HPHT + R)-l») 

DET=1/TEMP3(1,1) 

C DO 27 1*1, L 

C27 WRITE(8,92) DET 

CALL CONST(DET,TEMP,N,L,G,ND,LD) 

WRITE(8,99) 

99 FORMATC/* MATRIX G *) 

DO 3024 1*1, N 

3024 WRITEC8,92)CGCI,J),J*1,L) 

C NOTE HERE PKKCI,J) * PCK/K) WHERE PCK/K) * CI-GCK)»H)XPCIC/K-1) 

CALL PRODCG,H,N,L,N,TEMP,ND,MD,LD) 

C WRITEC8,30) 

C 

30 FORMATC* GH * ) 

C DO 25 1*1, N 

C25 WRITEC8,92)CTEMPCI,J),J*1,N) 

C WRITEC8,37) 

37 FORMATC* IDENTITY MATRIX *) 

C DO 45 1*1, N 

C45 WRITEC8,92)C UNITCI, J), J=1,N) 

C 

C 

CALL SUBCUNIT, TEMP, N,N, TEMPI, ND,MD) 

WRITEC8,33) 

33 FORMATC* I -GH * ) 

DO 35 1=1, N 

35 WRITEC8,92)CTEMP1CI,J),J*1,N) 

CALL PR0DCTEMP1,PICICM1,N,N,N,PKK,ND,MD,LD) 

FLAG*0.0 

RETURN 
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END 

C 

C 

SUBROUTINE QFINDCDT, GAM, XKKMl>N>Q>N,ti,ND,MD, SIGMA, K^MFLAG^GFLAG) 
C XXX PURPOSE XXX 

C CALCULATES THE STATE EXCITATION COVARIANCE OF ERROR MATRIX 
C 

c 

c XXX VARIABLE DEFINITIONS XXX 

C SAME AS MAIN PROGRAM, EXCEPT 

C SIGVT2*(0.0I KTS/SEOXX2 * A10.8 YDSXX2/MINXX4 

C SIGTH2=(0.IDEG/SEC)XX2 « 0.01096 RADSXX2/MINXX2 

C SIGF02-(0.001HZ/SEC)XX2- 0.0036 HZXX2/MINXX2 

C CALC W MATRIX WHERE W- E(W(K)XW' (K) ) 

C W(l,l)s(SIGX)xx2 

C W(2,2)=(SIGY)xx2 

C W(1,2)=(SIGXY) 

C H(3,3)=(SIGF0)xx2*SIGF02 

C 

C XXX VARIABLE DECLARATIONS xxx 

DIMENSION GAM(5,5),GAMT(5,5),W(5,5),Q(5,5),TEMP(5,5) 

DIMENSION XKKM1(5),SIGMA(3) 

C SET UP W MATRIX 

IFCGFLAG.NE.l.) THEN 
IF(MFLAG.EQ.l) THEN 
C MFLAG = 0 NO MANUEVERING 

C MFLAG = 1 MANUEVERING 

SIGVT2 =A10.8 
SIGTH2 = 0.01096 
SIGF02 = 0.0036 
SIGMA(1)=SIGVT2 
SIGMA(2)=SIGTH2 
SIGMA(3)=SIGF02 
C 
C 
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EVT2 = XKKMl(2)**2+XKKMl(‘i)»»2 

W(l,l)=((XKKMl(2)»»2)/EVT2)»SIGVT2+(XKKMl(<i)**2)»SIGTH2 
W(2,2) = ((XKKMl(<i)»»2)/EVT2)»SIGVT2+(XKKMl(2)»»2)*SIGTH2 
W(l,2)*((XKKMl(2)»XKKMl(4))/EVT2)»SIGVT2+(XKKMl(2)»XKKMl(«i))Jf 
tSIGTH2 
N(2a)=N(l,2) 

H(3,3)-SIGF02 

ELSE 

H(1U)=10. 

W(2,2)*10. 

N(3,3)-0.01 

ENDIF 

ENDIF 

543 HRITE(8,544) 

544 FORMATC/* W •) 

DO 3021 1=1,3 

3021 WRITE(8,92) (W(I, J ) , J=1 , 3) 

C 

CALL TRANS(GAM,N,M,GAMT,ND,MD) 

C NRITE(8,1) 

1 F0RMAT(/>5X, • GAMT MATRIX •) 

C DO 100 1=1, M 

ClOO NRITE(8,92) (GAMTd, J), J = 1,N) 

92 F0RMAT(2X,8(1PE12.5,2X)) 

CALL PROD(GAM,H,N,M,N,TEMP,ND,MD,MD) 

C NRITE(8,2) 

2 F0RMAT(/,5X, • TEMP MATRIX •) 

C DO 101 1=1, N 

ClOl WRITE(8,92) (TEMPCI, J) , J=1,M) 

CALL PROD(TEMP,GAMT,N,M,N,Q,ND,MD,MD) 

C REMOVE C*S FOR PRINTOUT IF DESIRED 

C WRITE(8,3) 

3 F0RMAT(/,5X, • Q MATRIX •) 

C DO 102 1=1, N 

C102 NRITE(8,92) (Q(I, J), J=1,N) 
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RETURN 



END 

C 

C 

C 

C 

SUBROUTINE FMEAS(XB, YB, XKKMl , VP, R, N, NB, K, FDKKMl , D) 

C XXX PURPOSE XXX 

C SUBROUTINE CALCS THE H MATRIX FOR THE FREQ. MEASUREMENTS 
C AND SELECTS AND R. 

C 

C XXX VARIABLE DECLARATIONS xxx 

DIMENSION XB(10),YB(10),XKKM1(5),H(5,5) 

C FREQ NOISE STD DEV SFREQ-0 . OAHZ. 

SFREQ* O.OA 

RSSFREQXX2 

HRITE(8,A0) 

40 FORMATC/* R FOR FREQ •) 

HRITE(8,92) R 

C NRITE(8,41) 

41 FORMAT(/» VP *) 

C HRITE(8,92) VP 

92 F0RMAT(2X,8(1PE12.5,2X)) 

U=(((XKKM1(1)-XB(NB))XXKKM1(2))+((XKKM1(3)-YB(NB))XXKKM1(4))) 
C NRITE(8,32) 

32 FORMAT (/• U •) 

C NRITE(8,92) U 

D=((XKKMl(l)-XB(NB))xx2+(XKKMl(3)-YB(NB))xx2)xx0.5 
C NRITE(8,43) 

43 FORMAT(/» D *) 

C WRITE(8,92) D 

H(1,5)=1/(1+(U/(VPXD))) 

AK=(XKKM1(5)X(H(1,5)XX2))/(VPXDXD) 

C WRITE(8,34) 

34 FORMATC/' AK •) 



lai 



c 



MRITE(8,92) AK 

H(1,1)=-AKX((XKKM1(2)XD)-(U»(XKKM1(1)-XB(NB))/D)) 

H ( 1 , 2 ) s-AKXDX( XKKMl ( 1) -XB ( NB ) ) 

H(l,3)s-AKX((XKKM1(A)»D)-(U*(XKKM1(3)“YB(NB)))/D) 
H(1,A)=-AKXDX(XKKM1(3)-YB(NB)) 

FDKKM1=XKKM1(5)XH(1,5) 

C NRITE(S.AA) 

FORMAK/* FDKKMl *) 

C NRITE(8,92) FDKKMl 

C 

RETURN 

END 

C 

C 

C 

C 

SUBROUTINE BMEAS(XB» YB, XKKMl , VP, R. N» NB» K» BRKKMl ) 

C SUBROUTINE CALCS THE H MATRIX FOR THE BEARING MEASUREMENTS 
C AND SELECTS AND R. 

DIMENSION XB(10),YB(10), XKKM1(5),H(5,5) 

PI180=0. 0174533 

C BEARING NOISE STD DEV SDBRG=5 DEGS. 

SDBRG=5.0 

R=(SDBRGXPI180)XX2 

NRITE(8,40) 

40 FORMAT(/* R FOR BEAR. ») 

WRITE(8,92) R 
A=XKKM1(1)-XB(NB) 

B=XKKM1(3)-YB(NB) 

D=SQRT(A»»2+B»*2) 

92 F0RMAT(2X,8(1PE12.5,2X)) 

C WRITE(8,32) 

32 F0RMAT(/,5X, *A*,12X, *B') 

C NRITE(8,92) A,B 

C NRITE(8,43) 
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C 



C 

C 



C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

C 

c 

c 

c 

c 

c 

c 

c 



FORMATC/* D *) 

WRITE(8,92) D 

H(1,1)=B/(DKD) 

H(l,2)=0.0 

H(1,3)=-A/(DXD) 

H(1^A)=0.0 

H(l,5)-0.0 

CALL BEARCATS, BRKKMl) 
WRITE(8,A4) 

FORMATC/* BRKICMl *) 
HRITE(8^92) BRKKMl 
RETURN 
END 



SUBROUTINE PL0TF(TIME,XT,YT,G,PICIC,IC,NB,NBU0Y,N2,MZ2,VARX,VARY) 
XXX PURPOSE XXX 

CONTAINS GRAPHIC DATA FOR KALMAN GAINS. COVARIANCE MATRIX. AND 
ERROR ELLIPSOIDS FOR FREQ MEAS 



XXX VARIABLE DEFINITIONS XXX 

TERMS SAME AS MAIN PROGRAM. EXCEPT FOR 



ELLIP 

EVARX 

EVARY 

GF 

GX 

GXD 

GY 

GYD 

PF 

PVV 

PX 



SUBROUTINE TO CALC ERROR ELLIPSOIDS 
MATRIX OF VARX 
MATRIX OF VARY 

MATRIX OF FREQ GAINS. STORE FOR PLOTTING 
MATRIX OF X COMP GAINS. STORE FOR PLOTTING 
MATRICX OF VX COMP GAINS 

MATRIX OF Y COMP GAINS. STORE FOR PLOTTING 

MATRIX OF VY COMP GAINS 

MATRIX OF FREQ COMP COVARIANCE OF ERROR 

USED FOR PLOTTING ERROR ELLIPSOIDS 

X COMP VARIANCE. USED WITH ERROR ELLIPSOIDS 
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c 


PXD 


= 


VX COMP VARIANCE, 




c 


PXY 




COVARIANCE OF X AND Y 


COMPS 


c 


PY 


= 


Y COMP VARIANCE, USED 


HITH ERROR ELLIPSOIDS 


c 


PYD 




VY COMP VARIANCE 





c 

c XXX VARIABLE DECLARATIONS xxx 

DIMENSION TIME(200),XT(200),YT(200),G(5,5),PKK(5,5) 

DIMENSION YH(120),XH(120),XP(200),YP(200) 

DIMENSION GX(6>200),GXD(6,200)^GY(6.200)>GYD(6/200),GF(6,200) 
DIMENSION PX(6,200)>PXD(6,200),PY(6,200),PYD(6,200),PF(6,200) 
DIMENSION PXY(6,200),PVV(6,200),EVARX(6>200)>EVARY(6,200) 

C HRITE(7,1) NB 

C NRITE(9a) NB 

Cl F0RMAT(/,5X,* FREQ.MEAS. FROM BUOY M2) 

KK=K-l 

IF(K.GT.MZZ) GO TO 888 
GX(NB,K)-G(1>1) 

GXD(NB,K)=G(2,1) 

GY(NB,K)sG(3,l) 

GYD(NB,K)=G(A,1) 

GF(NB,K)=G(5,1) 

C SET UP PLOTS OF PKKMl COMPONENTS 

PX(NB,K)=PKK(1,1) 

PXD(NB,K)=PKK(2,2) 

PY(NB,K)=PKK(3,3) 

PYD(NB,K)=PKK(4,4) 

PF(NB,K)=PKK(5,5) 

PXY(NB,K)=PKK(1,3) 

PVV(NB,K)=PKK(2,4) 

EVARX(NB,K)=VARX 

EVARY(NB,K)=VARY 

PFLAG=0.0 

C COMMENT OUT ONE OF CALLS IN EACH IF-THEN, THE 
C FIRST CALL ELLIP COMPUTES POSITION ERROR ELLIPSES, SECOND ONE 
C THE VELOCITY ERROR ELLIPSES 
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c 



IF(K.EQ.l) THEN 

CALL ELLIP(XT,YT,PX,PY,PXY,K,NB,PFLAG) 

C CALL ELLIP(XT,YT,PXD,PYD,PVV,K,NB,PFLAG) 

ELSEIF(K.EQ.ll) THEN 

CALL ELLIP(XT,YT,PX,PY,PXY,K,NB,PFLA6) 

C CALL ELLIP(XT,YT,PXD,PYD,PVV,K,NB,PFLA6) 

ELSEIF(K.EQ.31) THEN 

CALL ELLIP(XT,YT,PX,PY,PXY,K,NB»PFLA6) 

C CALL ELLIP(XT,YT,PXD,PYD,PVV,K,NB,PFLAG) 

ELSEIF(K.EQ.61) THEN 

CALL ELLIP(XT,YT,PX,PY,PXY,K,NB,PFLA6) 

C CALL ELLIP(XT,YT,PXD,PYD,PVV,K,NB,PFLAG) 

ENDIF 
GO TO 900 
C 

888 DO 6001 I^l.NBUOY 
WRITE(7,80) I 
WRITE(9,81) I 
WRITE(19,81) I 

80 F0RMAT(/,5X, • FREQ MEAS. GAINS FROM BUOY SI2) 

81 FORMAT!/, 5X, • FREQ MEAS. VAR. FROM BUOY M2) 

DO 6002 J=NZ,KK 

WRITE(7,95) TIME(J),GX(I,J),GXD(I,J),GY(I,J),GYD(I,J),GF(I,J) 
WRITE(9,95) TIME(J),PX(I,J),PXD(I, J),PY(I,J),PYD(I,J),PF(I,J) 
WRITE! 19, 95) TIME! J ) ,PX!I, J) , PY! I, J) , PXY!I, J) 

IF!J.GE.ll) THEN 

WRITE! 12, 95) TIME! J) , EVARX! I, J) , EVARY!I, J) ,PX!I, J) ,PY!I , J) 
ENDIF 

6002 CONTINUE 
6001 CONTINUE 

95 F0RMAT!/F7.2,2X,5!1PE12.5,2X)) 

C95 F0RMAT!/,F8.2,<iX,F11.3,4X,F11.3,AX,F11.3,AX,F11.3) 

900 RETURN 




END 



c 

c 

c 

SUBROUTINE PLOTBCTIME, XT, YT, 6, PKK, K, NB, NBUOY, NZ, MZZ, BFLAG, VARX, 
XVARY) 

C nun PURPOSE nnn 

C CONTAINS GRAPHIC DATA FOR KALMAN GAINS, COVARIANCE MATRIX, AND 
C ERROR ELLIPSOIDS FOR BEARING MEAS 

C 

C XXX VARIABLE DEFINITIONS XXX 

C SAME AS MAIN PROGRAM AND/OR PLOTF 
C 

C XXX VARIABLE DECLARATIONS xxx 

DIMENSION TIME(200),XT(200),YT(200),G(5,5),PKK(5,5) 

DIMENSION BFLAG(10),GF(6,200),PF(6,200) 

DIMENSION GX(6,200),QXD(6,200),GY(6,200),GYD(6,200) 

DIMENSION PX(6,200),PXD(6,200),PY(6,200),PYD(6,200) 

DIMENSION PXY(6,200),EVARX(6,200),EVARY(6,200) 

C WRITE(7,1) NB 

C WRITE(9,1) NB 

Cl F0RMAT(/,5X, • BEARING MEAS. FROM BUOY ',12) 

KK=K-1 

IF(K.GT.MZZ) GO TO 888 
GX(NB,K)=G(1,1) 

GXD(NB,K)=G(2,1) 

GY(NB,K)=G(3,1) 

GYD(NB,K)=G(4,1) 

GF(NB,K)=G(5,1) 

C SET UP PLOTS OF PKKMl COMPONENTS 

PX(NB,K)=PKK(1,1) 

PXD(NB,K)=PKK(2,2) 

PY(NB,K)=PKK(3,3) 

PYD(NB,K)=PKK(4,4) 

PF(NB,K)=PKK(5,5) 

PXY(NB,K)=PKK(1,3) 
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EVARX(NB,K)=VARX 
EVARY(NB,K)=VARY 
PFLAG=1.0 
IF(K.EQ.l) THEN 

CALL ELLIP(XT,YT,PX,PY,PXY,K,NB,PFLA6) 

ELSEIF(K.EQ.ll) THEM 

CALL ELLIP(XT,YT,PX,PY,PXY,K,NB,PFLA6) 

ELSEIF(K.EQ.31) THEM 

CALL ELLIP(XT,YT,PX,PY,PXY,K,NB,PFLA6) 

ELSEIF(K.EQ.61) THEM 

CALL ELLIP(XT,YT,PX,PY,PXY,K,NB,PFLAG) 

ENDIF 
GO TO 900 
C 

BBS DO 6001 I-l,NBUOY 

C DON^T PRINT GAINS AND VAR IF ITS A LOFAR BUOY (THERE 0 ANYHAY) 
IF(BFLAG(I).EQ.2.) GO TO 6001 
WRITE(7,B0) I 
WRITE(9,S1) I 
WRITE(19,S1) I 

50 F0RMAT(/,5X,* BEARING MEAS. GAINS FROM BUOY *,I2) 

51 F0RMAT(/,5X, * BEARING MEAS. VAR. FROM BUOY *,I2) 

DO 6002 J=NZ,KK 

WRITE(7,95) TIME(J),GX(I,J),GXD(I,J),GY(I,J),GYD(I,J),GF(I,J) 
HRITE(9,95) TIME(J),PX(I,J),PXD(I, J),PY(I,J),PYD(I,J),PF(I,J) 
HRITE(19,95) TIMEC J ) , PX( I , J ) , PY( I , J ) , PXY( I , J ) 

IF(J.GE.ll) THEN 

HRITE(12,95) TIME(J),EVARX(I, J),EVARY(I,J),PX(I,J),PY(I,J) 
ENDIF 

6002 CONTINUE 
6001 CONTINUE 

95 F0RMAT(/F7.2,2X,5(1PE12.5,2X)) 

900 RETURN 
END 
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c 

c 

c 

SUBROUTINE ELLIP(XT, YT, PI , P3, P13, K, MB,PFLAG) 

C XXX PURPOSE XXX 

C XXX ROUTINE TO PLACE ELLIPSE DATA IN FILE 
C 

C XXX VARIABLE DECLARATIONS xxx 

DIMENSION XT(200),YT(200),XP(200),YP(200) 

DIMENSION P1(6.200),P3(6,200),P13(6.200) 

A=2XP13(NB,K) 

B=P1(NB,K)-P3(NB,K) 

IF((A.EQ.O.O).AND.(B.EQ.O.O)) B-O.OOOl 
THE1-.50XATAN2(A,B) 

A=(Pl(NB,K)+P3(NB,K))/2. 

B^O.O 

IF(P13(NB,K).EQ.0.0) GO TO 10 
B=P13(NB,K)/SIN(2.xTHE1) 

10 SIG2X=(A+B) 

SIG2Y=(A-B) 

SX=((SIG2X)xx.5) 

• SY=((SIG2Y)xx.5) 

PT=3. 14159265/12 
CT=C0S(THE1) 

ST=SIN(THE1) 

C WRITE(4,9897) K,NB 

9897 FORMAT(//» xxxxx SUMMARY FOR K= *,I4,» FROM BUOY *,I2,*xxxxx») 
IFCPFLAG.EQ.l.) THEN 

C WRITE(4,9898) 

9898 FORMATC/* BEARING MEAS . ERROR ELLIPSE •) 

ELSE 

C WRITE(4,9999) 

9999 FORMATC/* FREQ. MEAS. ERROR ELLIPSE *) 

ENDIF 

DO 1981 IELLIP=1,25 
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XI=IELLIP 

XP( I EL L IP ) =SX*COS( PTXXI ) »CT-SY»SI N( PT»XI ) *ST+XT( K ) 

YP ( 1 EL L IP ) =SX*COS ( PTXXI ) »ST+SY*SIN ( PTXXI ) »CT+YT( K ) 

1981 MRITE(<i,1982)XP(IELLIP),YP(IELLIP) 

1982 F0RMAT(2F14.4) 

RETURN 

END 

C XXX END OF ELLIPSE CALCULATION 
C 
C 
C 

SUBROUTINE PLOTGF(TIME,K, NB, NBUOY, NZ,MZ, E, GATES, TGATE, RFLAG) 
C XXX PURPOSE XXX 

C GRAHICS INPUT FILE ADAPT GATE AND PREDICTED 

C RESIDUAL FROM FREQ NEAS 

C 

C XXX VARIABLE DEFINITIONS XXX 

C SAME AS MAIN EXCEPT FOR 

C ERR s MATRIX TO STORE PREDICTED RESIDUALS 

C TRIP s MATRIX OF NUMBER OF TIMES GATES IS 

C EXCCEDED 

C 

C XXX VARIABLE DECLARATIONS xxx 

DIMENSION ERR(6,200),GATE(6,200),TIME(200) 

DIMENSION TRIP(5,200),RESTR(5,200) 

C 

C 

IF(K.GT.MZ) GO TO 888 

ERR(NB,K)=E 

GATE(NB,K)=GATES 

TRIP(NB,K)=TGATE 

RESTR(NB,K)=RFLAG 

GO TO 900 

888 DO 6000 I=l,NBUOY 
HRITE(18,80) I 
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80 F0RMAT(/,5X, *FREQ. RESIDUAL AND GATE FROM BUOY SI2) 

DO 6001 J=NZ,MZ 

WRITE(18,95) TIME(J),TRIP(I,J),RESTR(I,J),ERR(I,J),GATE(I,J) 
6001 CONTINUE 
6000 CONTINUE 

95 F0RMAT(F7.2,2X,F5.2,2X,F5.2,2X,2(1PE12.5,2X)) 

900 RETURN 
END 
C 
C 
C 

SUBROUTINE PLOTGB(TIME,K,NB»NBUOY,NZ,MZ,E, GATES, TGATE^RFLAG) 

C PLOTGB s SUBROUTINE TO PLOT ADAPT GATE AND PREDICTED 

C RESIDUAL FROM BEARING MEAS 

C GRAHICS INPUT FILE ADAPT GATE AND PREDICTED 
C RESIDUAL FROM BEARING MEAS 
C 

C XXX VARIABLE DEFINITIONS xxx 



c 


SAME AS 


MAIN 


EXCEPT FOR 


c 


ERR 


s 


MATRIX TO STORE PREDICTED RESIDUALS 


c 


TRIP 


s 


MATRIX OF NUMBER OF TIMES GATES IS 


c 






EXCCEDED 


c 


RESTR 


= 


MATRIX OF THE NUMBER OF RESTARTS FOR EACH 


c 






MEAS 



C XXX VARIABLE DECLARATIONS XXX 

DIMENSION ERR(6,200),GATE(6,200),TIME(200) 
DIMENSION TRIP(5,200),RESTR(5,200) 

C 

C 

IF(K.GT.MZ) GO TO 888 

ERR(NB,K)=E 

GATE(NB,K)=GATE3 

TRIP(NB,K)=TGATE 

RESTR(NB,K)=RFLAG 
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GO TO 900 

888 DO 6000 I-lrNBUOY 
HRITE(18,80) I 

80 F0RMAT(/»5X, 'BRG. RESIDUAL AND GATE FROM BUOY SI2) 

DO 6001 J-NZ,MZ 

WRITE(18,95) TIME(J),TRIP(I, J),RESTR(I,J),ERR(I, J),GATE(I,J) 
6001 CONTINUE 
6000 CONTINUE 

95 F0RMAT(F7.2,2X,F5.2,2X,F5.2,2X,2(1PE12.5,2X)) 

900 RETURN 

END 

C 

C 

c 

SUBROUTINE ZGATEC E, GATE, R, W, GFL AG, KOUNT, GATES, TGATE) 

C XXX PURPOSE XXX 

C CALCS THE GATES, AND RANDOM FORCING FUNC COV MATRIX VALUES 
C 

C XXX VARIABLE DEFINITIONS xxx 
C SAME AS MAIN PROGRAM, EXCEPT FOR 
C GATEl s ONE SIGMA- (GATE + R) xxo.5 
C 

C XXX VARIABLE DECLARATIONS XX 
DIMENSION H(5,5) 

GATEl=(GATE+R)xxo.5 
GATES=SXGATE1 
HRITE(8,96) GATES 

96 FORMATC/, 'GATES = •,1PE12.5) 

IF(ABS(E)-GATES.GT.O.) THEN 
GFLAG=1. 

TGATE =TGATE+1 

K0UNT=K0UNT+1 

N(1,1):=10.XW(1,1) 

W(2,2)=10.XH(2,2) 

W(S,S)=2XW(S,S) 
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WRITE(8,97) 

97 FORMAK/,* GATE3 HAS BEEN EXCEEDED ') 
ENDIF 
RETURN 
END 
C 
C 
C 
C 

SUBROUTINE RSTARTCXB, XKKMl , PKKMl ) 

C XXX PURPOSE XXX 

C REINITIALIZES THE PROGRAM 

C 

C XXX VARIABLE DEFINITONS xxx 

C SAME AS MAIN PROGRAM 

C 

C XXX VARIABLE DECLARATIONS XXX 

DIMENSION XKKM1(5).PKKM1(5,5),XB(10) 

DO 118 1-1,5 
DO 118 J-1,5 
118 PKKM1(I,J)=0.0 

PKKMl (1,1)=1.025E6 
PKKM1(3,3)=PKKM1(1,1) 

PKKM1(2,2)=1.025E4 

PKKM1(A,A)=PKKM1(2,2) 

PKKM1(5,5)=1.0 

RETURN 

END 

C 

C 

c 

c 

SUBROUTINE GAUSS(DSEED,SIG,MEAN,Z,NFLAG) 
C XXX PURPOSE XXX 

C GAUSSIAN PSEUDO- RANDOM NUMBER GENERATOR 
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c 

c 

c XXX VARIABLE DECLARATIONS XXX 

DOUBLE PRECISION DSEED 
REAL MEAN 

IF(NFLAG.EQ.l) THEN 
NR^l 

TEMP=0.0 

DO 10 I-ia2 

CALL GGUBS(DSEED,NR^R) 

TEMP=TEMP+R 
10 CONTINUE 

Z=(TEMP-6 . 0)XSIG+MEAN 

ELSE 

Z^MEAN 

ENDIF 

C WRITE(6,92) (MEAN,Z(I),I*1,NR) 

C92 F0RMAT(/,2X, *MEAN= *,F8.3,* Z* »,F8.3) 

RETURN 

END 

C 

C 

C 

SUBROUTINE GGUBS (DSEED, NR, R) 

C XXX PURPOSE XXX 

C BASIC UNIFORM (0,1) PSEUDO-RANDOM NUMBER GENERATOR 

C 

C 

C XXX VARIABLE DECLARATIONS xxx 

INTEGER NR 

REAL R(NR) 

DOUBLE PRECISION DSEED 
C SPECIFICATIONS FOR LOCAL VARIABLES 

INTEGER I 

DOUBLE PRECISION D2P31M,D2P31 
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C D2P31M=(2»K31) - 1 

C D2P31 *(2X»31)(0R AN ADJUSTED VALUE) 

data D2P31M/21<i7A836A7.D0/ 

DATA D2P31/21A7A836A8.D0/ 

C FIRST EXECUTABLE STATEMENT 

DO 5 I-1,NR 

DSEED * DMOD( 16807. D0XDSEED»D2P31M) 

5 R(I) * DSEED / D2P31 
RETURN 
END 
C 
C 
C 
C 

SUBROUTINE ADD(A, B,N,M,C, ND,MD) 

C XXX PURPOSE XXX 

C SUBROUTINE ADDS TWO MATRICES 

C XXX VARIABLE DECLARATIONS XXX 

DIMENSION A(ND,ND),B(ND,MD).C(ND>MD) 

C 

DO 152 I * 1,N 

DO 152 J = 1,M 

152 C(I,J) = A(I,J) + B(I,J) 

RETURN 

END 

C 

C 

c 

c 

SUBROUTINE SUB( A, B, N,M^ C, ND, MD) 

C XXX PURPOSE XXX 

C SUBROUTINE SUBTRACTS TWO MATRICES 

C XXX VARIABLE DECLARATIONS xxx 

DIMENSION A(ND,MD),B(ND,MD),C(ND,MD) 
DO 152 I = 1,N 
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152 



DO 152 J - 1,M 
C(I,J) * A(I,J) - B(I,J) 
RETURN 
END 
C 
C 
C 
C 



SUBROUTINE PROD(A,B,N,M, L^C^NO.MD^ LD) 



C 

C 

C 



1 
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XXX PURPOSE XX* 

SUBROUTINE MULTIPLES TWO MATRICES 
XXX VARIABLE DECLARATIONS XXX 

DIMENSION A(ND,MD),B(MD,LD),C(ND,LD) 
DO 1 I - l.N 
DO 1 J - 1,L 
C(I,J) = 0. 

DO 151 I - 1,N 
DO 151 J = 1,L 
DO 151 K - 1,M 

C(I,J) sC(I,J) + A(I,K)XB(K,J) 

RETURN 

END 



C 

C 

C 

C 



SUBROUTINE TRANSCA, N, M,C, ND, MD) 
C XXX PURPOSE XXX 

C SUBROUTINE TRANSPOSES A MATRIX 

C XXX VARIABLE DECLARATIONS XXX 

DIMENSION A(ND,MD),C(MD,ND) 

DO 153 I = 1,N 

DO 153 J = 1,M 

153 C(J,I) = A(I,J) 

RETURN 
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END 



C 

C 

C 

C 

SUBROUTINE CONSTCQ/ A,N,M/C,ND»MD) 

C XXX PURPOSE XMM 

C SUBROUTINE MULTIPLES A MATRIX BY A CONSTANT 
C XXX VARIABLE DECLARATIONS xxx 

DIMENSION A(ND/MD)/C(ND,MD) 

IF(Q) 11/10,11 



10 


DO 100 


I » 1/N 




DO 100 


J * 1/M 


100 


C(I,J) 


- 0.0 




RETURN 




11 


IF (Q-1 


.0) 13/12/ 


12 


DO 120 


I = 1/N 




DO 120 


J a 1/M 


120 


C(I/J) 


= ACI/J) 




RETURN 




13 


IF (Q+1 


.0) 15/14/ 


14 


DO 140 


I = 1/N 




DO 140 


J = 1/M 


140 


C(I/J) 


= -A(I/J) 




RETURN 




15 


DO 150 


I a 




DO 150 


J a 1/M 


150 


C(I/J) 


a QXACI/J) 



RETURN 

END 



C 

C 
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c 



c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 

c 



c 



c 



SAMPLE OF GRAPHICS PROGRAM. 

XXX PURPOSE XKK 

THIS PROGRAM PLOTS THE TARGET'S ACTUAL TRACK, THE FILTER 
ESTIMATED TRACK, THE BUOY PATTERN AND THE ERROR ELLIPSOIDS 

XXX VARIABLE DEFINITIONS xxx 

MAJORITY OF THE VARIABLES ARE SAME AS IN THE MAIN PROGRAM 
XP = ERROR ELLIPSOIDS X COMP 

YP s ERROR ELLIPSOIDS Y COMP 

MZZ = NUMBER OF TRACK POINTS 

NN s NUMBER OF ERROR ELLIPSOID P0INTS(25 POINTS/ELLIPSE) 

DISSPLA TERMS ARE DEFINE IN THE DISSPLA BOOK 



XXX VARIABLE DECLARATIONS xxx 

REAL T(200),XT(200),YT(200),EXT(200),EYT(200),OFF(200) 
REAL XP(500),YP(500) 

REAL BFLAG(6),XB(6),YB(6) 

MZZ=60 
NN=525 
NE=NN/25 
XORIG^IO.O 
XMAX=25.0 
YORIG=15,0 
YMAX=30,0 
CALL TEK618 



CALL COMPRS 
CALL NOBRDR 
CALL PAGE(8.5,11. ) 
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CALL HEIGHTC0.15) 

CALL MXIALFC'STANDARTS'S') 

CALL MX2ALF(*L/CSTD*, •#•) 

C CALL PHYS0RC2. 0,2.0) 

CALL AREA2DC6. 0,6.0) 

C GRAPHICS ROUTINE TO TRACK, BUOYS AND ERROR ELLIPSES 

C 

CALL INTAXS 

CALL XNAME(*KYARDS$»,100) 

CALL YNAME(*KYARDS$*,100) 

CALL GRAFCXORIG, ‘SCALE* , XMAX,YORIG, * SCALE* ,YMAX) 

DO 10 I-1,NE 
DO 11 J=l,25 
READ(4,X) XP(J),YP(J) 

XP(J)=XP(J)/1000.0 
YP(J)=YP(J)/1000.0 
11 CONTINUE 

CALL DASH 

CALL CURVE(XP,YP,25,0) 

10 CONTINUE 

READ(<i,») (XT(I),YT(I),EXT(I),EYT(I),0FF(I),I = 1,MZZ) 
READ(4,X) NBUOY 

READ(4,X) (BFLAG(I),XB(I),YB(I),I=1,NBU0Y) 

DO 1 1=1, MZZ 
XT(I)=XT(I)/1000.0 
YT(I)=YT(I)/1000.0 
EXT(I)=EXT(I)/1000.0 
EYT(I)=EYT(I)/1000.0 
1 CONTINUE 

CALL HEIGHTC0.15) 

CALL RESET(*DASH*) 

CALL MARKER(16) 

CALL CURVE(XT,YT,MZZ,5) 

CALL DASH 
CALL MARKER(A) 
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CALL CURVE(EXT,EYT,MZZ,5) 

CALL HEIGHT(O.IS) 

DO 1000 I=1,NBU0Y 
XB(I)=XB(I)/1000. 

YB(I)*YB(I)/1000. 

TITX=XB(I)-1 

TITY=YB(I)+1 

IF(BFLAG(I).EQ.l.) THEN 

CALL RLMESSC *DIFAR$MOO,TITX,TITY) 

CALL MARKER(16) 

CALL SCLPICC2) 

CALL CURVE(XB(I),YB(I),1,5) 

ENDIF 

IF(BFLAG(I).EQ.2.) THEN 
CALL RLMESSC *LOFAR$MOO,TITX,TITY) 
CALL MARKER(16) 

CALL SCLPICC2) 

CALL CURVE(XB(I),YB(I),1»5) 

ENDIF 

1000 CONTINUE 

CALL ENDPL(O) 

CALL DONEPL 

STOP 

END 
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C XXX PURPOSE XXX 

C SAMPLE GRAPHICS PROGRAM PLOTS THE PREDICTED RESIDUAL, 

C A-DAPTIVE GATE, AND THE NUMBER OF TIMES THE GATE IS EXCEEDED 

C FIRST THE PROGRAM PLOTS THE GRAPHS FOR THE FREQ MEAS, THEN ON 

C ANOTHER PAGE PLOTS THE GRAPHS FOR THE BEARING MEAS. 

C HENCE AS SHOMN IN FIG. 6-10 

C 

C XXX VARIABLE DEFINITIONS XXX 

C SAME AS MAIN PROGRAM, AND STANDARD DISSPLA TERMS 

C 

C DRAM RESIDUAL AND GATE FOR FREQ MEASUREMENTS 

REAL T(500),ERR(500),GATE(500),TRIP(500),FLAG(500) 

XORIG-0.0 
XMAX=60.0 
C X0RIG=60.0 

C XMAX=l<iO.O 

YORIGsO.O 
YMAXsS.O 

C CALL TEK618 

CALL COMPRS 
CALL NOBRDR 
CALL PAGE(11.,8.5) 

CALL HEIGHTC0.15) 

CALL MXIALFC 'STANDART*, *8*) 

CALL MX2ALF(*L/CSTD», •#*) 

READ(4,x) (T(I),TRIP(I),FLAG(I),ERR(I),GATE(I),I=1,60) 

C READ(4,x) (T(I),TRIP(I),FLAG(I),ERR(I),GATE(I),I=1,81) 

CALL PHYSORC .75,5.0) 

CALL AREA2D(<». 0,2.75) 

CALL INTAXS 
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CALL XNAME( *TIME(MINS)$MOO) 

CALL YNAME( 'RESIDUAL & GATE MEAS$',100) 

CALL GRAF(XORIG, 'SCALE' ,XMAX,YORIG, 'SCALE' ,YMAX) 

CALL HEIGHT(0.15) 

CALL RLMESSCBUOY 1$ ' , 100 ,70 . 0 , YMAX) 

CALL HEIGHT(0.15) 

CALL THKCRV (0.01) 

CALL CURVE(T, GATE, 60,0) 

CALL CURVE(T, GATE, 81,0) 

CALL DASH 

CALL CURVE(T, ERR, 60,0) 

CALL CURVE(T, ERR, 81,0) 

CALL RESET ('DASH') 

CALL MARKER(2) 

CALL CURVE(T, TRIP, 60,-1) 

CALL CURVE(T, TRIP, 81,-1) 

CALL RESET ('MARKER') 

CALL ENDGR(O) 

XORIG-0.0 

XMAX=60.0 

X0RIG=60.0 

XMAX=140.0 

Y0RIG=0.0 

YMAX=3.0 

READ(4,») (T(I),TRIP(I),FLAG(I),ERR(I),GATE(I),I=1,60) 
READ(4,») (T(I),TRIP(I),FLAG(I),ERR(I),GATE(I),I=1,81) 
CALL PHYSOR(6.5,5.0) 

CALL AREA2D(4. 0,2.75) 

CALL HEIGHT(0.15) 

CALL XNAME( 'TIME(MINS)$',100) 

CALL YNAME( 'RESIDUAL 8 GATE MEAS$',100) 

CALL GRAF(XORIG, 'SCALE' ,XMAX,YORIG, 'SCALE' ,YMAX) 

CALL HEIGHT(0.15) 

CALL RLMESSCBUOY 2$ ', 100,70 . ,YMAX) 

CALL HEIGHT(0.15) 
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c 



c 



c 



c 



CALL THKCRV (0.01) 

CALL CURVECT, GATE, 60,0) 

CALL CURVEd, GATE, 81,0) 

CALL DASH 

CALL CURVE(T, ERR, 60,0) 

CALL CURVECT, ERR, 81,0) 

CALL RESETCDASH*) 

CALL MARKER(2) 

CALL CURVECT, TRIP, 60,-1) 

CALL CURVE(T, TRIP, 81,-1) 

CALL RESET( 'MARKER*) 

CALL ENDGR(O) 

XORIG^O.O 

XMAX-60.0 

XORIG-60.0 

XMAX*140.0 

YORIG-0.0 

YMAX=3.0 

READ(4,X) (T(I),TRIP(I),FLAG(I),ERR(I),GATE(I),I=1,60) 
READ(<*,X) (T(I),TRIP(I),FLAG(I),ERR(I),GATE(I),I = 1,81) 
CALL PHYSOR(.75,1.0) 

CALL AREA2D(4. 0,2.75) 

CALL HEIGHT(0.15) 

CALL XNAME(*TIME(MINS)$*,100) 

CALL YNAMEC 'RESIDUAL 8 GATE MEAS$*,100) 

CALL GRAF(XORIG, 'SCALE* ,XMAX,YORIG, 'SCALE* ,YMAX) 

CALL HEIGHTC0.15) 

CALL RLMESSCBUOY 3$*,100,70. ,YMAX) 

CALL HEIGHT(0.15) 

CALL LINESP (2.0) 

CALL LINES ('FREQ GATE$ * , IPAK, 1 ) 

CALL LINES ( 'RESIDUALS *, IPAK, 2) 

XW=XLEGND (IPAK,2) 

YW=YLEGND (IPAK,2) 

CALL LEGLIN 
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CALL 


THKCRV (0.01) 




CALL 


CURVE(T, GATE, 60,0) 


c 


CALL 


CURVE(T, GATE, 81,0) 




CALL 


DASH 




CALL 


CURVE(T, ERR, 60,0) 


c 


CALL 


CURVE(T, ERR, 81,0) 




CALL 


RESETCDASH' ) 




CALL 


MARKER(2) 




CALL 


CURVE(T, TRIP, 60,-1) 


c 


CALL 


CURVE(T, TRIP, 81,-1) 




CALL 


RESET ('MARKER') 




CALL 


LEGEN0(IPAK,2,6.,2. 


c 








CALL 


ENDPL(O) 



XORIG-0.0 
XMAX=60.0 
C XORIG-60.0 

C XMAXslAO.O 

Y0RIG=0.0 
YMAX=1.0 
CALL NOBRDR 
CALL PAGE(11.,8.5) 

CALL HEIGHTCO.IS) 

READ(4,X) (T(I),TRIP(I),FLAG(I),ERR(I),GATE(I),I=1,60) 
CALL PHYSORC .75,5.0) 

CALL AREA2D(A. 0,2.75) 

CALL INTAXS 

CALL XNAME(*TIME(MINS)$*,100) 

CALL YNAMEC 'RESIDUAL 8 GATE MEAS$*,100) 

CALL GRAFCXORIG, 'SCALE* ,XMAX,YORIG, 'SCALE* ,YMAX) 

CALL HEIGHTC0.15) 

CALL RLMESSCBUOY 1$ *, 100,70 . 0,YMAX) 

CALL HEIGHTC0.15) 

CALL THKCRV (0.01) 

CALL CURVE(T, GATE, 60,0) 
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c 



c 



c 



c 

c 



c 

c 



c 



c 



c 



CALL CURVEd, CATE, 81,0) 

CALL DASH 

CALL CURVECT, ERR, 60,0) 

CALL CURVE(T, ERR, 81,0) 

CALL RESET(*DASH*) 

CALL MARKER(2) 

CALL CURVEd, TRIP, 60,-1) 

CALL CURVE(T, TRIP, 81,-1) 

CALL RESET(*MARKER*) 

CALL ENDGR(O) 

PLOT FOR BUOY 2 
PLOT NEXT GRAPH 
XORIG-0.0 
XMAX=60. 

X0RIG=60.0 

XMAX=1A0.0 

Y0RIG=0.0 

YMAX=1.0 

READ(A,») (T(I),TRIP(I),FLAG(I),ERR(I),GATE(I),I=1,60) 
READ(A,X) (T(I),TRIP(I),FLAG(I),ERR(I),GATE(I),I=1,81) 
CALL PHYSOR(6.5,5.0) 

CALL AREA2D(4.0,2.75) 

CALL HEIGHTCO.IS) 

CALL XNAME(*TIME(MINS)$',100) 

CALL YNAMEC 'RESIDUAL & GATE MEAS$',100) 

CALL GRAFCXORIG, 'SCALE' ,XMAX,YORIG, 'SCALE' ,YMAX) 

CALL HEIGHTCO.IS) 

CALL RLMESSCBUOY 2$ ' , 100, 10 . , YMAX) 

CALL RLMESSCBUOY 2$ ', 100,70 . ,YMAX) 

CALL HEIGHTCO.IS) 

CALL THKCRV CO. 01) 

CALL CURVECT, GATE, 60,0) 

CALL CURVECT, GATE, 81,0) 

CALL DASH 

CALL CURVECT, ERR, 60,0) 
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C CALL CURVEd, ERR, 81,0) 

CALL RESET(»DASH») 

CALL MARKER(2) 

CALL CURVEd, TRIP, 60,-1) 

C CALL CURVEd, TRIP, 81,-1) 

CALL RESET ('MARKER*) 

C CALL LEGENDdPAK, 2,1. ,2.25) 

CALL ENDGR(O) 

C PLOT FOR BUOY 3 

C PLOT NEXT GRAPH 

XORIG-0.0 
XMAX=60. 

C X0RIG=60.0 

C XMAX=1A0.0 

Y0RIG=0.0 
YMAX=1.0 

READ(A,30 (T(I),TRIP(I),FLAG(I),ERR(I),GATE(I),I*1,60) 
C READ(A,X) (T(I),TRIP(I),FLAG(I),ERR(I),GATE(I), 1*1,81) 

CALL PHYSOR( .75,1.0) 

CALL AREA2DCA. 0,2.75) 

C CALL INTAXS 

CALL HEIGHT(0.15) 

CALL XNAMEC *TIME(MINS)$*,100) 

CALL YNAME( 'RESIDUAL 8 GATE MEAS$*,100) 

C CALL CROSS 

CALL GRAFCXORIG, 'SCALE* ,XMAX,YORIG, 'SCALE' ,YMAX) 

CALL HEIGHTC0.15) 

C CALL RLMESSCBUOY 3$ * , 100, 10 . , YMAX) 

CALL RLMESSCBUOY 3$ * , 100, 70 . , YMAX) 

CALL HEIGHTC0.15) 

CALL LINESP (2.0) 

CALL LINES CBRG GATE$ ' , IPAK, I ) 

CALL LINES ( 'RESIDUALS ', IPAK, 2) 

CALL LINES (* EXCEEDS *, IPAK, 3 ) 

XW=XLEGND (IPAK,2) 
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YW=YLEGND (IRAK, 2) 

C CALL MYLEGN (»BUOY 3$ MOO) 

CALL LEGLIN 
CALL THKCRV (0.01) 

CALL CURVEd, GATE, 60, TRIP) 

C CALL CURVECT, GATE, 81,0) 

CALL DASH 

CALL CURVECT, ERR, 60,0) 

C CALL CURVECT, ERR, 81,0) 

CALL RESET(»DASH») 

CALL MARKERC2) 

CALL CURVECT, TRIP, 60,-1) 

C CALL CURVECT, TRIP, 81,-1) 

CALL RESET(» MARKER*) 

CALL LEGEND(IPAK,2,6.,2.25) 
C 

CALL ENDPLCO) 

CALL DONEPL 

STOP 

END 
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